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ABSTRACT 


Today’s modern avionics systems rely heavily on the integration of Global Posi¬ 
tioning System (GPS) data and the air vehicle’s accelerations obtained by an Inertial 
Measurement Unit (IMU). To properly resolve the GPS and the IMU data, one must 
have an understanding of the different coordinate systems involved. Since the GPS 
provides data in one coordinate reference frame and the IMU measures accelerations 
in another, transforming the data freely from one frame to the next is imperative if 
the avionics system is to provide meaningful data to the aircrew. 

This thesis provides a uniform approach to analysis and design of an inte¬ 
grated GPS/IMU avionics system using MMYLAB!Simulink software development 
tools. Topics covered include: 

• Coordinate Systems and Transformations 

• Fundamentals of Inertial Sensors 

• Tangent Plane Navigation 

• Kinematic Equations and Error Analysis 

• Global Positioning System (GPS) Sources and Errors 

• Kalman Filter Design 

Emphasis is placed on addressing the theory and providing detailed examples 
to support each topic. 




VI 




TABLE OF CONTENTS 


I. INTRODUCTION. 1 

II. COORDINATE SYSTEMS . 3 

A. TRUE INERTIAL {/}. 3 

B. EARTH-CENTERED-INERTIAL {i} . 3 

C. EARTH-CENTERED-EARTH-FIXED {e}. 4 

D. GEODITIC COORDINATE SYSTEM. 5 

E. TANGENT PLANE COORDINATE SYSTEM. 5 

F. NAVIGATION COORDINATE SYSTEM {n} . 6 

G. WANDER AZIMUTH COORDINATE SYSTEM {c}. 7 

H. PLATFORM FRAME {p} . 8 

I . ACCELEROMETER FRAME {a}. 8 

III. COORDINATE TRANSFORMATIONS. 11 

A. EULER ANGLES. 11 

B. GEODITIC TO ECEF COORDINATE TRANSFORMATION . . 16 

C. ECEF TO GEODITIC COORDINATE TRANSFORMATION . . 17 

D. ECEF TO TANGENT PLANE COORDINATE TRANSFORMA¬ 
TION . 18 

E. TANGENT PLANE TO ECEF COORDINATE TRANSFORMA¬ 
TION . 19 

F. VECTOR NOMENCLATURE. 20 

G. TRANSFORMATION OF ANGULAR VELOCITIES. 22 

H. SPECIFIC TRANSFORMATIONS. 23 

1 . ECI to ECEF: 23 

vii 























26 


2. ECEF to Navigation: ^. 

3. ECEF to Wander Azimuth {c}: 27 

4. Navigation to Body: 30 

I. COORDINATE FRAMES FOR INS MECHANIZATIONS .... 31 

J. PLATFORM MECHANIZATIONS. 36 

K. PLATFORM MISALIGNMENT. 38 

L. ESTIMATE OF PLATFORM TO NAVIGATION TRANSFORMA¬ 
TION: "C . 39 

IV. INERTIAL SENSORS. 41 

A. LASER GYRO FUNDAMENTALS: PASSIVE SAGNAC INTER¬ 
FEROMETER . 41 

B. FIBER OPTIC GYRO . 42 

V. TANGENT PLANE NAVIGATION. 45 

A. TANGENT PLANE EQUATIONS. 45 

B. GENERAL NAVIGATION EQUATIONS. 46 

C. ERROR ANALYSIS. 49 

D. THE VERTICAL CHANNEL. 49 

E. ATMOSPHERIC MODEL . 51 

F. VERTICAL CHANNEL DAMPING: COMPLEMENTARY FILTER¬ 
ING . 53 

VI. KINEMATIC EQUATIONS AND ERROR ANALYSIS. 59 

A. ROTATING COORDINATES . 59 

B. GRAVITATIONAL MODEL. 65 

C. SPACE STABILIZED MECHANIZATION. 71 

D. ERROR ANALYSIS. 79 

VII. NAVIGATION USING NAVAIDS. 91 

viii 


























VIII. THE NAVSTAR GPS . 101 

A. SPACE SEGMENT. 101 

B. USER SEGMENT. 105 

C. CONTROL SEGMENT. 106 

D. DIFFERENTIAL GPS . 107 

E. GPS ERROR SOURCES.. . . 107 

1. Atmospheric Delays. 108 

a. Ionospheric Delays . 108 

b. Tropospheric Delays . Ill 

2. Selective Availability. 112 

3. Clock Differences. 113 

4. Ephemeris Error . 116 

5. Multipath. 118 

6. Receiver Noise. 119 

7. Dilution of Precision. 119 

F. INERTIAL NAVIGATION. 121 

1. GimbaledIMU. 122 

2. Strapdown IMU. 123 

G. INS COMPUTATIONS. 123 

H. INS ERROR SOURCES. 125 

1. Biases. 125 

2. Cross-Axis Sensitivity. 125 

3. Noise Floor. 126 

IX. KALMAN FILTER DESIGN . 127 

X. GPS/IMU INTEGRATION. 137 

A. EQUATIONS OF MOTION . 137 

ix 





























B. INERTIAL MEASUREMENT UNIT (IMU). 141 

C. GLOBAL POSITIONING SYSTEM (GPS). 143 

D. KALMAN FILTER. 144 

1. Navigation Model. 144 

2. Linearized Model. 146 

3. Synthesis Model. 146 

4. Noise Intensity and the Non-Linear Plant . 147 

XL CONCLUSION. 153 

APPENDIX A: SHOWCASE, SNAPSHOT, AND XV UNIX SOFTWARE 155 

A. IRIS SHOWCASE 3.3. 155 

1. Creating 1-D and 3-D Objects. 156 

2. Saving the Drawing. 159 

3. Encapsulated Postscript. 160 

4. Postscript. 160 

5. Additional 3D Goodies. 161 

B. SNAPSHOT. 161 

C. XV. 163 

D. SHOWCASE PRESENTATIONS. 164 

APPENDIX B: MATLAB FUNCTIONS . 191 

LIST OF REFERENCES. 211 

INITIAL DISTRIBUTION LIST. 213 


X 























LIST OF TABLES 


8.1 ALLAN VARIANCE PARAMETERS FOR THREE COMMON TIM¬ 


ING STANDARDS . 116 

10.1 EIGENVALUES . 140 


XI 






xii 





LIST OF FIGURES 


2.1 Earth-Centered, Earth-fixed Coordinate System. 4 

2.2 Geodetic Coordinate System. 6 

2.3 Tangent Plane Coordinate System. 7 

2.4 Wander Azimuth, Navigation and ECEF Coordinate Systems. 8 

3.1 Collocated Universal {u} and Body {6} Coordinate Systems. 12 

3.2 Rotation About the 2 : Axis. 13 

3.3 Trigonometric Relationships Between Rotated Frames. 13 

3.4 Rotation About the y' Axis . 14 

3.5 Trigonometric Relationships Between Rotated Frames. 15 

3.6 ECEF Spherical Model with Altitude. 19 

3.7 Position Vectors of Point Q . 21 

3.8 ENU Navigation Frame. 23 

3.9 Earth’s Sidereal Rate. 24 

3.10 Sidereal Rate in the Equatorial Plane. 25 

3.11 Point on Earth’s Surface. 25 

3.12 ECEF to Navigation. 26 

3.13 ECEF to Wander Azimuth {UEN} . 28 

3.14 Wander Azimuth (NEU). 29 

3.15 Wander Azimuth (UEN). 29 

3.16 NED Orientation. 30 

3.17 Positive Positive 6, Positive ^. 31 

3.18 Torque Applied to Maintain Orientation. 32 

3.19 Geographic and Inertial Torque Model. 33 

xiii 


























3.20 loe . 34 

3.21 UN . 34 

3.22 uu . 35 

3.23 36 

3.24 Platform Frame {p} with Angular Rate, a . 37 

4.1 Ideal Rotating Sagnac Interferometer [Ref. 1, p. 87]. 41 

4.2 Fiber Optic Gyro [Ref. 1, p. 119]. 43 

5.1 Tangent Plane. 45 

5.2 Block Diagram of Inertial Navigation System. 48 

5.3 Altitude Definitions [Ref. 1, p. 199] . 50 

5.4 Complementary Filter Mechanization. 54 

5.5 Complementary Filter: ^-Channel. 55 

6.1 Rotating Coordinates. 59 

6.2 Calculation of Velocity and Position . 63 

6.3 Standard Mechanization. 68 

6.4 Latitude and Longitude in {i} Frame. 72 

6.5 Navigation Mechanization —> ”1/ . 73 

6.6 Wander Azimuth Navigation. 75 

6.7 U and N Transformation. 76 

6.8 Tangent Plane Mechanization. 79 

6.9 Rec . 80 

6.10 Tangent Mechanization: Vertical Channel . 82 

6.11 Vertical Channel with Error. 83 

6.12 Vertical Channel Complementary Filter. 83 

6.13 Linearized Vertical Channel Filter. 84 

6.14 Random Process . 85 


XIV 






























6.15 Gaussian Distribution . 88 

7.1 Two Bearing Fix. 92 

7.2 Error Estimation.. 92 

7.3 Orthogonal Fixes. 94 

7.4 Dependent Fixes . 94 

7.5 Maximum Likelihood Mechanization . 98 

7.6 Unit Vector, . 100 

8.1 A NAVSTAR GPS Satellite. 102 

8.2 Diurnal Ionospheric Delay. 109 

8.3 Receiver Clock Model. 113 

8.4 Ideal Allan Variance. 114 

8.5 Real Allan Variance. 115 

8.6 Ephemeris Error . 117 

8.7 Dilution of Precision. 120 

8.8 GimbaledIMU . 122 

9.1 Synthesis Model. 128 

9.2 Kalman Filter Model. 129 

9.3 Tangent Plane Mechanization. 130 

9.4 Body Frame and Tangent Plane Relationship. 130 

9.5 Pseudorange. 131 

9.6 User Equivalent Range Error Mechanization. 132 

10.1 Equations of Motion Simulink Block Diagram. 140 

10.2 Inertial Measurement Unit (Level 1) Simulink Block Diagram. 141 

10.3 Inertial Measurement Unit (Level 2) Simulink Block Diagram. 142 

10.4 GPS Pseudo-range Simulink Block Diagram. 143 

10.5 Navigation Model Simulink Block Diagram. 145 


XV 





























10.6 Kalman Filter, Pseudo-range Simulink Block Diagram. 145 

10.7 Synthesis Model for Kalman Filter, Simulink Block Diagram. 147 

10.8 Non-Linear Kalman Filter. 148 

10.9 Frequency Response, Pseudo-range to Pseudo-range Estimate 1 & 2 . 149 
lO.lOFrequency Response, Pseudo-range to Pseudo-range Estimate 3 &; 4 . 150 

10.11 Frequency Response, Accelerations to Pseudo-range Estimate. 150 

10.12Pseudo-range Response to Step Input in Aj;, Aj,, . 151 

A.l Showcase Drawing Tablet . 155 

A.2 Showcase Master Gizmo. 156 

A.3 Showcase Status Gizmo. 156 

A.4 Showcase 3D Gizmo . 157 

A.5 Showcase 3D Drawing Window . 158 

A.6 Showcase 3D Sphere. 159 

A.7 Showcase 3D Sphere in ID Environment. 160 

A.8 Showcase File Browser. 161 

A.9 Showcase PS Save Window. 161 

A.10 Showcase Page Gizmo . 162 

A.11 SGI Snapshot Window. 162 

A.12 XV Window. 163 

A. 13 XV Controls Window. 163 

A.14 XV Save Window. 164 

A.15 Coordinate Transformation, Slide 1. 165 

A.16 Coordinate Transformation, Slide 2. 166 

A. 17 Coordinate Transformation, Slide 3. 167 

A. 18 Coordinate Transformation, Slide 4. 168 

A. 19 Coordinate Transformation, Slide 5 .. 169 


XVI 



























A.20 Coordinate Transformation, Slide 6. 170 

A.21 Coordinate Transformation, Slide 7. 171 

A.22 Coordinate Transformation, Slide 8. 172 

A.23 Coordinate Transformation, Slide 9. 173 

A.24 Navigation to Body Rotation, Slide 1. 174 

A.25 Navigation to Body Rotation, Slide 2. 175 

A.26 Navigation to Body Rotation, Slide 3. 176 

A.27 Navigation to Body Rotation, Slide 4. 177 

A.28 Navigation to Body Rotation, Slide 5. 178 

A.29 Navigation to Body Rotation, Slide 6. 179 

A.30 Navigation to Body Rotation, Slide 7. 180 

A.31 Navigation to Body Rotation, Slide 8. 181 

A.32 Navigation to Body Rotation, Slide 9. 182 

A.33 Navigation to Body Rotation, Slide 10. 183 

A.34 Navigation to Body Rotation, Slide 11 . 184 

A.35 Navigation to Body Rotation, Slide 12. 185 

A.36 Navigation to Body Rotation, Slide 13. 186 

A.37 Navigation to Body Rotation, Slide 14. 187 

A.38 Navigation to Body Rotation, Slide 15. 188 

A.39 Navigation to Body Rotation, Slide 16. 189 


xvii 






















XVlIl 



ACKNO WLED GMENT 


Many thanks to Prof. Kaminer for his knowledge of, guidance with, and en¬ 
thusiasm for the subject material. I owe a debt of gratitude to my fellow officers 
in the Aero 611 curriculum, their wit and wisdom entertained and encouraged me 
throughout my time at NPS. Most importantly, without the support of my family; 
Debbie, Jessica and Amanda, this effort would not have been possible. Thanks for 
your love and patience! 


XIX 



I. INTRODUCTION 


The research conducted in the Avionics lab at the Naval Postgraduate School 
(NPS) Aeronautical Engineering Department, centers around the flight control of 
Unmanned Autonomous Vehicles (UAV’s). Today’s battlefield has become increas¬ 
ingly hostile to manned airborne vehicles as weapons have become more accurate 
and lethal. In order to reduce the risk, while still providing forward reconnaissance, 
the military has increasingly turned toward remotely operated aircraft. Autonomous 
vehicles, allow the commander in the field the opportunity to survey his environment 
and employ the UAV as a targeting, weapons, or intelligence gathering platform. 
The reduced risk to the pilot and the decreased cost when compared to a current fleet 
aircraft, makes the deployment of UAV’s a high priority in future battles. 

In order to control the vehicle in a variety of battlefield environments, precise 
knowledge of it’s velocity and position is essential. Using on-board inertial navi¬ 
gation systems, the vehicle’s accelerations can be precisely measured and used by 
on-board navigational computers to calculate the vehicle’s velocity and position in 
some particular reference frame. An inertial navigation system, alone, can provide 
accurate position information in the short term, but must be integrated with an ad¬ 
ditional source if precise positional data is to be maintained for the long term. The 
UAV’s requirement to operate aboard ships or in rugged locations makes it essential 
that extremely accurate position information is calculated. To meet this requirement 
an inertial navigation system can be integrated with an Global Positioning System 
(GPS) receiver. This provides the vehicle’s navigation system with an additional, 
highly accurate positioning data source. 
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The purpose of this thesis is to provide the framework for designing an integrated 
inertial measurement / GPS system. To understand the system integration as a whole, 
it is necessary to have a solid understanding of the reference frames in which both 
systems operate and how the angular and translational motion of the vehicle relative 
to these reference frames is calculated. In an effort to make the three dimensional 
(3-D) relationships and the associated mathematics more understandable, numerous 
3-D graphical examples are provided. 

A brief overview of current inertial sensors; ring laser gyros and fiber optic gyros, 
is included in order to understand how vehicle accelerations are measured and how 
they are related to the body of vehicle. 

As with any system, the IMU errors are inherent in the IMU’s computations. 
Detailed analysis of errors is essential if the IMU accuracy is to be fully realized [Ref. 
1, p. 231]. Since the IMU is not a position fixing system, incorporating a GPS as 
the fixing source and using a Kalman filter to combine the two, provides the best 
estimate of the vehicle’s velocity and position. Kalman filter design will be discussed 
and developed using MATLAB/5imw/m^ software development tools. 
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II. COORDINATE SYSTEMS 


To develop the relationship between GPS and the IMU, an understanding of 
each coordinate system is essential. In this chapter, eight coordinate frames will be 
discussed with particular attention paid to only four. They are the Earth-Centered- 
Inertial (ECI) coordinate system, the Earth-Centered-Earth-Fixed (ECEF) coordi¬ 
nate system, the tangent plane coordinate system and the Wander Azimuth coordi¬ 
nate system. 

A. TRUE INERTIAL {/} 

The True Inertial coordinate system is a set of mutually perpendicular axes 
that neither accelerate nor rotate with respect to some fixed point in space. Newton 
assumed there was a reference frame whose absolute motion was zero (fixed relative 
to the stars) [Ref. 1, p. 9], and it is in this reference frame where they are valid. 
However, Newton’s laws of motion can also be applied to any reference frame as long 
as the proper coordinate transformations are applied. 

B. EARTH-CENTERED-INERTIAL {i} 

The Earth-Centered-Inertial (ECI) coordinate system is centered about the 
origin of the earth and maintains a fixed orientation with respect to some inertial 
reference in space. As the earth rotates the frame stays oriented with respect to 
this inertial reference. Even though the ECI frame is referenced to the earth and 
translates as the earth rotates about the sun, it’s rotational motion relative to the 
fixed point is space is minimal and can be ignored. This allows the assumption that 
Newton’s laws of motion apply here also. In the cases we will discuss, we will assume 
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the ECI frame stays fixed and all other frames will rotate with respect to the ECI 
frame. 

C. EARTH-CENTERED-EARTH-FIXED {e} 

The Eaxth-Centered-Earth-Fixed coordinate system, is similar to the ECI sys¬ 
tem except that the frame itself is connected to earth, that is, it rotates with the earth. 
Every twenty-four hours the ECEF frame coincides with the ECI frame. The system 
origin, as the name implies, is centered at the Earth’s origin. The X-axis is directed 
through the intersection of the Greenwich meridian, 0® longitude and the equator, 0° 
latitude. The Y-axis is directed from the origin to intersect the equator at 90° east 
longitude. The Z-axis is directed along the Earth’s spin axis from the origin through 
the north pole at 90° north latitude. The ECEF frame is depicted in Figure 2.1. The 



Figure 2.1: Earth—Centered, Earth—fixed Coordinate System 

earth-centered earth-fixed system is independent of the mathematical model of the 
earth’s surface. However, both the geodetic and the local geodetic systems depend on 
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the specification of the earth model. The current standard for modeling the surface of 
the earth is the WGS-84 ellipsoid. This ellipsoid is generated by rotating an ellipse, 
whose semi-major axis is 6378137.0 meters and whose se mi -minor axis is 6356752.3 
meters, about its minor axis. The resulting closed surface is the model of the earth’s 
surface. The true north pole (conventional terrestrial pole) and true south pole are 
the endpoints of the minor axis of the ellipsoid. 

D. GEODITIC COORDINATE SYSTEM 

The output of navigation systems used on aircraft today is generally latitude, 
longitude, and altitude — i.e. resolved in the geodetic coordinate system. This is the 
system used for describing positions of most earth bound objects. Charts developed 
for long range land and sea navigation invariably use geodetic coordinates. 

The geodetic coordinate system is somewhat analogous to spherical coordinate 
system. The primary difference is that the elevation angle or latitude, (p, is the angle 
between the ellipsoidal normal and the equatorial plane. This means that the ray 
that defines this angle does not intersect the equatorial plane at the exact center of 
the earth. Instead, it intersects the equatorial plane at a small radius outside of the 
center as shown in Figure 2.2. 

The longitude. A, is identical to the spherical concept of that angle. It is the 
angle in the equatorial plane from 0° latitude and 0° longitude to any given point. 
Finally, h, the geodetic height or altitude, is the distance along the ellipsoidal normal 
away from the surface of the earth. 

E. TANGENT PLANE COORDINATE SYSTEM 

Typically, pure inertial systems navigate in a so-called tangent plane coordinate 
system, before outputting position in geodetic coordinates. The tangent plane system 
is defined by passing a plane at any point on the earth’s surface. The intersection of 
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Figure 2.2: Geodetic Coordinate System 

the plane with the surface of the earth becomes the origin of the system. The x-axis 
points toward true east. The j/-axis points toward true north. Lastly, the ^-axis is 
perpendicular to the defining plane of the system, away from the center of the earth. 
It is the z coordinate of the triad which defines a point’s altitude in this system. This 
frame is sometimes referred to as the universal {u} frame. This is shown in Figure 2.3. 

F. NAVIGATION COORDINATE SYSTEM {n} 

The navigation coordinate system is attached to the aircraft’s frame. It’s ori¬ 
gin is located at the center of the aircraft’s inertial navigation system [Ref. 1]. It 
maintains a local-level orientation to the reference ellipsoid in the same manner as 
the tangent plane but the orientation of the axes can be chosen at random. If the 
origin of the aircraft is co-located at the origin of the tangent plane, then the tangent 
plane and the navigation plane are one in the same. The frame shown in Figure 2.3, 
represents an East-North-Up (ENU) orientation where the x-axis points east, the 
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Figure 2.3: Tangent Plane Coordinate System 

j/-axis points north and the 2 - axis points up. The orientation of the frame can be 
specified any number of ways; North-East-Up {NEU) or North-East-Down {NED). 

G. WANDER AZIMUTH COORDINATE SYSTEM {c} 

The Wander Azimuth coordinate system is a frame centered at some reference 
point on the body, most likely the center of the aircraft’s inertial system, the origin 
of the navigation frame. The x and y plane is tangent to the local vertical with the 
2 axis perpendicular. The frame is defined with respect to the ECEF frame using 
longitude A, latitude (j) and the wander azimuth angle a. The angle o is positive when 
the frame is to the west of true north, the angle A is positive east of the Greenwich 
meridian, and (j) is positive north of the equatorial plane. If the wander azimuth frame 
is aligned with the ECEF frame at the Greenwich meridian, the angle a is zero. The 
Wander Azimuth, co-located tangent and navigation planes and the ECEF frames 
are depicted in Figure 2.4. 
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Figure 2.4: Wander Azimuth, Navigation and ECEF Coordinate Systems 
H. PLATFORM FRAME {p} 

The platform coordinate system is the right-handed orthogonal frame defined by 
the input axes of the inertial sensors. If the inertial navigation device is a “strapdown” 
model, the aircraft’s accelerations are measured in the platform frame. Imagine the 
inertial system, perhaps a ring laser gyro, bolted to the frame of the aircraft and all 
accelerations experienced by the aircraft are directly measured by the sensors of the 
inertial unit. Platform accelerations are equivalent to body fixed accelerations if the 
unit is a “strapdown system”. In this case the platform frame would be referred to 
as the body fixed frame {b}. This assumption will be valid throughout this thesis. 

L ACCELEROMETER FRAME {a} 

The accelerometer frame is a non-orthogonal frame defined by the sensitive axes 
of the accelerometers which make up the inertial navigation system [Ref. 2]. If the 
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axes of the inertial measurement unit are not properly aligned with the platform, 
errors in acceleration measurements can exist [Ref. 1]. 
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III. COORDINATE TRANSFORMATIONS 


The kinematic equations that relate the motion of a body relative to a fixed 
reference frame must be given in terms of the relative angular velocity and coordinate 
transformations between each frame. In order to use these coordinate systems, one 
must be able to transform between them freely. For example, satellite positions for 
the GPS computations are given in ECEF coordinates. Suppose the aircraft posi¬ 
tion is wanted in the tangent plane coordinate system. Therefore, a transformation 
between ECEF and tangent plane systems is required. For the three systems above, 
six conversions are required. By being able to convert from geodetic to ECEF, and 
ECEF to local geodetic, one can convert from geodetic to local geodetic by chaining 
the two conversions together. Thus, the only four transformations discussed are: 

• geodetic to ECEF 

• ECEF to geodetic 

• ECEF to tangent plane 

• tangent plane to ECEF 

A. EULER ANGLES 

Euler angles are used to define the orientation of two coordinate systems with 
respect to each other. For example, consider the aircraft body fixed coordinate system 
{6} and how it is oriented with respect to the tangent plane or universal frame {u}. 
We define the angles the aircraft’s body makes with this system in terms of roll <t>, 
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pitch and yaw ih. Written in vector format: 


■ 4 >' 

A= e (3.1) 

. ^ - 

Figure 3.1 represents body and universal coodinate frames where the origins are 
collocated. 
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Figure 3.1: Collocated Universal {it} and Body (6) Coordinate Systems 

X 

Given a vector ?/, resolved in {u}: = y , what are the components of y 

z 

in {6} or y{l This can be accomplished using Euler angles as follows. First, let’s 
rotate the {«} frame about the 2 :-axis. Since the angle V’ describes rotation about 
the 2 axis, our first rotation matrix will be a function of i\>. Figure 3.2 depicts the 
rotation about the z-axis. The z-axis should be considered positive outward from the 
x-y plane. The rotation is considered positive using the right-hand-rule. 

The new axes of the rotated frame have been labeled (x', y', 2 '). Since the 
transformation matrix describes the trigonometric relationships between the old and 
new frames let’s use Figure 3.3 to determine what these relationships are. 
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The same relationship exists for y'. In this case the xsin'ip component is in the 
direction opposite of our sign convention. Therefore y' written as a function of the 
components of x and y becomes: 


y'= —X sin-tp + y costp (3-3) 

Since the rotation occurred about the sr-axis, z' = z. Gathering the terms in matrix 
format we have the following transformation matrix: 

x' COS0 simf^ 0 1 r a: 

y' = — sin0 cosip 0 y . (3.4) 

_ z' \ [ 0 0 1 J [ J 

Let’s now perform the rotation about the new y' axis. This time the rotation 
angle is 6 and the resulting axes will be {x", y", z"). Figure 3.4 shows the relationship 
between the new axes. 



Figure 3.4: Rotation About the y' Axis 


Using the same process we used to calculate {x',y',z'), we develop a transfor¬ 
mation matrix based on the trigonometric relationships between the new axes. These 
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relationships are shown in Figure 3.5. 



z' 

'''• z'sinS 

Figure 3.5: Trigonometric Relationships Between Rotated Frames 

The new transformation matrix becomes: 

x" cos6 0 — sin0 x' 

y" ^ 0 1 0 y' . (3.5) 

z" _ sin0 0 COS0 z' 

Notice that the negative sign now appears in the first row with the sin term. 
This is due to the fact that the rotation was through the angle ^ in the positive 
direction, or nose up. The x” component, -^'sin^, was in the direction opposite of 
the sign convention chosen. 

The last transformation will be about the x" axis which is a rotation of 4>i 
right wing down. This is the final orientation of the body frame and results in a 
transformation matrix: 
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' x'" 1 r 0 0 1 1 r 

y'" = 0 cos<f) sm(l) y" 

z'" 0 — silK^ COS(/> z" 

Combining equations 3.4, 3.5, and 3.6, we have the final (3-2-1) transformation 
from (x, y, z) to {xb, yt, Zb) as; 

X 10 0 cos^ 0 — sin^ cosip sinip 0 x 

y = 0 cos(p sind 0 10 — sin^ cosip 0 y 

^ \ b L 0 0 0 1 z 

(3.7) 

When performing transformations, the Euler angles associated with each rota¬ 
tion are not vectors. That is, ^ -f ^ 7 ^ ^ -|- ^. This implies that the order of rotation 
is important. In the previous example, the rotation order was 3-2-1, or ip,0,p. To 
verify, perform a 2-3-1, or ip,(p,6 and compare the final transformation matrix with 
equation 3.7. 

B. GEODITIC TO ECEF COORDINATE TRANSFORMATION 

In order to compute the transformation from geodetic to ECEF coordinates, 
three auxiliary quantities — /, e, and N — must first be defined [Ref. 5]. The 
flattening factor, /, represents the relative flatness of the ellipsoid. A zero flattening 
factor would mean an unflattened ellipse (a sphere), while a unity value would mean 
a totally flattened ellipsoid (a circle in the plane perpendicular to the minor axis). 
The mathematical definition of / is 

/=—. (3.8) 

a 

where a and b are the semi-major and semi-minor axes of the ellipsoid, respectively. 
Directly related to the flattening factor is the eccentricity, e. It is defined by 

£2 = 2/ - f. (3.9) 
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The eccentricity is a variable similar to the flattening factor. It represents how close 
the ellipsoid is to a sphere. It, too, is zero for a sphere and one for a completely 
flat figure. The eccentricity, rather than the flattening factor, is typically used in 
coordinate transformations. 

Lastly, N is the length of the ellipsoidal normal from the ellipsoidal surface to 
its intersection with the ECEF z-axis. Mathematically, N is 



where <f> is the geodetic latitude. 

Using these quantities, one may define the transformation 


(3.10) 


X = {N + h) cosc/) cosA 
y = (N + h) coscf) sinA 

^ = [AI(1-£^) +/i]sin^. (3.11) 


C. ECEF TO GEODITIC COORDINATE TRANSFORMATION 

The transformation from ECEF to geodetic coordinates is clearly the inverse of 
the process presented in the previous section. First, A, the longitude, can be found 
by dividing first two expressions of Equations 3.11 yielding 


tan A = 


(3.12) 


By examining the geometry in Figure 2.2, one can determine the following relationship 


tan (f> = 


{N + h) sin^ 

\/a;2 q. y2 


(3.13) 


which is a non-linear equation in (f>. Solving the third of Equations 3.11 for (iV + 
h)sin <i> and substituting into Equation 3.13 yields 


tan <j) = 


^ , e^Nsincj) 

-j- j/2 z ’ 


(3.14) 
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which is still an analytically unsolvable equation in geodetic latitude. To solve this 
equation, one initially assumes that h is zero, an excellent assumption for intra- 
atmospheric flight. Now, the z equation in Equation 3.11 can be simplified to 

-2/1=0 = -/V(l - e^)sin^. (3.15) 


This equation can be substituted into Equation 3.14 to give the initial solution for </> 


tan 61 


1 — -v/a:^ 4- 


(3.16) 


This initial solution for </> can be substituted back into the second term of Equa¬ 
tion 3.14 to yield an updated (f). Iteration of this process commencing with Equa¬ 
tion 3.14 continues until the geodetic latitude stops changing. Finally, solving Equa¬ 


tion 3.13 for h 


which completes the conversion. 


4 - 

cosS 


(3.17) 


If we assume a spherical earth model, a and b in Equation 3.8 are equal, resulting 
in a flattening factor / = 0. (Assume a = b = Ro, radius of the earth). Substituting 
/ in Equation 3.9, the eccentricity factor e also equals zero. Applying these values 
to Equation 3.10, the length of the ellipsoidal normal N is now simply the radius of 
the earth Ro. Substituting these values in equations 3.11 through 3.18, the altitude 


h can now be defined as 


_ p 

fl - j ito 

COS(p 


(3.18) 


The physical representation can be seen in Figure 3.6. 


D. ECEF TO TANGENT PLANE COORDINATE TRANSFORMATION 

Anytime one uses a local geodetic or tangent plane coordinate system, one must 
first specify the geodetic coordinates — latitude and longitude - of the origin. Once 
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Figure 3.6: ECEF Spherical Model with Altitude 


specified, the origin must be expressed in ECEF coordinates. Then, a vector from this 


origin to the point being transformed can be formed, resolved in the ECEF system 


Ax 


X2 — Xi 

Ay 

— 

2/2 -2/1 

. . 

e 

. ^2- Zl _ 


where the ‘2’ subscript denotes the point being transformed. 


(3.19) 


The product of two 


rotation matrices operates on the difference vector defined in Equation 3.19 to yield 


the local geodetic coordinates of the point where the subscript {«} represents the 


universal frame. 



X 


— sinA 

cos A 

0 ■ 


Ax 

Pu = 

y 

= 

— sin^ cosA — smcf) sinA 

COs4> 


Ay 


z 

u 

cos(l) cos A 

cos(j> sinA 

sin(f) 


. . 


where A is the geodetic longitude and <j) is the geodetic latitude. 


(3.20) 


E. TANGENT PLANE TO ECEF COORDINATE TRANSFORMATION 

The transformation from tangent plane to ECEF can be derived by merely 
reversing the process developed in the previous section. 
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First, the origin of the local geodetic system must be converted from geodetic 
to ECEF coordinates. Next, the inverse of the rotations performed in Equation 3.20 
must be executed yielding the A vector from the origin of the tangent plane system 
to the point being transformed. This vector, expressed in ECEF coordinates is 


Ax 


— sinA 

— sin<^ cosA 

cos<l> cos A 


X 


Ay 


cos A 

— sind* sinA 

cosi/' sinA 


y 


. . 

e 

0 

COS(j) 

sin^ 


z 

u 


(3.21) 


Clearly, adding the A vector to the position of the origin of the tangent plane system 


(both now in ECEF coordinates) completes the transformation: 


X 


1 

o 


Ax 

y 

= 

yorig 

+ 

Ay 

z 

e 

1 

o 

e 

. . 


F. VECTOR NOMENCLATURE 


(3.22) 


In order to proceed further, there must be a firm understanding of the nomen¬ 
clature used to define the vector of interest and the frame of reference where it is 
resolved. The vectors we will be interested in are position P, linear velocity V, and 
angular velocity lo. Each vector has a magnitude and direction and a reference frame 
to which it is attached. For the purpose of this discussion, we will be concerned only 
with two reference frames, the tangent (universal), denoted by u, and the body fixed 
frame, denoted by {6}. As we become more familiar with other reference frames we 
will use different characters. 

Let Q be an arbitrary point located on the body of the aircraft. We can draw a 
position vector to that point from the origin of the body reference frame. This vector 
would be identified as ^Pq . The P stands for a position vector, the subscript Q for the 
point of interest and the superscript b for the reference frame the point is measured 
with respect to. Simply put, ‘‘Pq is the position of point Q resolved in {6}. As with 
any vector, the position vector can be described in terms of any reference frame. An 
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example would be to define the point Q with respect to the universal frame or “Pq. 
Figure 3.7 illustrates the position vectors in both reference frames. 



Figure 3.7: Position Vectors of Point Q 

If the position vector P refers to the origin of a reference frame, the subscript 
will be Ox where o refers to the origin and x refers to the frame of interest. Therefore, 
'Pofc would refer to the position of the origin of the {b} frame, resolved in the universal 
frame {u}. 
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G. TRANSFORMATION OF ANGULAR VELOCITIES 


Now that we can describe the position of a point on a body with respect to an 
arbitrary reference frame, we also need to express the motion of the body as well. 
As the vehicle moves in inertial space, the angles that describe it’s relative position 
change as well. Question: How do we define the Euler angular rates in terms of the 
body’s angular rates? 

' <? ' 

A = 9 = ?a; =? 

- i’. 

Answer: Let Clbn be the angular velocity of {6} with respect to {n}. We can 
obtain A by considering the derivative of the Euler angle in the coordinate system it 
was defined. The total angular velocity is: 

0 0 ^ 

— C 0 + DB 0 D 0 (3.23) 

. ^ J L 0 J L 0 . 

Where C is the transformation matrix from {n} ^ {6}: 

COS0 sin^ 0 
- simp cosrp 0 
0 0 1 

DB is the transformation matrix from {n'} —> {6}, and D is the transformation 
matrix from {n"} —{6}. Combining terms results in: 

10 — sin^ ^ 

Lo — 0 cosd cos9 sin^ $ . (3.24) 

0 — sin^ COS0 cos^ ^ 

Finally, = jC5(a)), where: 

0 —IjJz U)y 

S{u;) = ujz 0 (3.25) 

_ —UJy Uz; 0 

is a skew symmetric matrix. 


10 0 cos9 0 — sin0 

C = 0 cos4> sincp 0 10 

0 — sin(f> cos(f> sin0 0 cos9 

' ----^- 

D B 
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H. SPECIFIC TRANSFORMATIONS 
1. ECI to ECEF: 

First, we need to determine Earth’s sidereal rate, flet The subscript ei im¬ 
plies the angular rate of {e} measured with respect to {f}. This convention is the 
reverse of the nomenclature in [Ref. 1] and will be maintained due to it’s simplic¬ 
ity. Assume the navigation frame and universal frame are co-located as shown in 
Figure 3.8. Consider Figure 3.9. It is clear from this figure that ftei resolved in {n} 


Zl, Ze 





Ye 


Yi 


Figure 3.8: ENU Navigation Frame 


is: 




0 

ricos^ 

Clsin(f> 


where f2 is the magnitude of Earth’s rotation and is given by: 

360° 


n = 


2Z + 


56 +^ 


60 


n = 15.041^ = 7.2 

hr sec 


(3.26) 
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Figure 3.9: Earth’s Sidereal Rate 

where one sidereal day equals 23hr 56min 4.09sec. 

Resolving the angular velocity in the ECEF {e} frame: 

'O' 

"flei = 0 (3.27) 

n 

This vector implies that the only component of flei, in the {e} frame is in the 2 
direction, see Figure 3.8. 

Now, consider the rotation viewed in the equatorial plane, where Zi = z^. 
Over a time At, the eaxth rotates at a rate flAt, figure 3.10. The transformation 
matrix from {t} to {e} can be calculated as: 

X cosOAt sinflAt 0 1 F a: 

y — — sinflAt cosflA 0 y (3.28) 

a: 0 0 1 z . 

LJeU JLJ^ 

Let A = Q{t — to) — 2n7r, where n is chosen such that 0 < A < 27r & (t — t^) is the time 
elapsed since vernal equinox. Since the two frames are orthogonal the transformation 
from {e} to {t} would be the inverse of the transformation matrix in Equation 3.28. 
Substituting A and inverting the matrix gives: 
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Figure 3.10: Sidereal Rate in the Equatorial Plane 


X 


’ cA —sA 0 


X 


y 


sA cA 0 


y 

(3.29) 

z 

i i 

1 

O 

O 


z 

e 


Finally, determine a point on the Earth’s surface in terms of latitude and longitude 
(^, A). Let P be the vector describing the point Q, Figure 3.11, then: 



Figure 3.11: Point on Earth’s Surface 
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(3.30) 




where R is the radius of the Earth. 


Rcos^cos A 
Rcos(f)sinX 
Rsm<f> 


2. ECEF to Navigation: ^ 

Now consider the transformation from {e} to {n}. Let the orientation of 
the navigation frame be: 


Xjj Uj Pn — E, Zji — N 



Figure 3.12: ECEF to Navigation 

Then 

• Rotating around Zg by A (longitude) 

• Rotating around j/e by <i> (latitude) 
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to obtain: 


II 

X 

y 

— 


z 

n L 


C(p 

0 

—sdi 


0 s(f) 
1 0 
0 c<f> 


cX sX 0 
—sX cX 0 
0 0 1 



X 


y 


z 


= 


(3.31) 


Note: Since the origins in Figure 3.12 are not coincident Eq. 3.31 is not completely 
correct. We need to include position of the origin of the navigation frame, 


vq = ‘p., + "Pq = :c(’Pq - •/>.,) 


Use vector calculus to define unit vectors for {n}: 




lE = 


Q,^iXR 




_ -Rx (Qet X-^) 

|Rx(ne:XR)| 




K 

\R\ 


3. ECEF to Wander Azimuth {c}: 

To obtain we must rotate {n} by an angle a around Xn'. 
where: 


c 

n 


c = 


10 O' 
0 ca $a 
0 —5a ca 


Then ^ see figure 3.13. 


(3.32) 


Now, for (n) = NEU 


\C^ 


ca —sa 0 
sa ca 0 
0 0 1 


(3.33) 
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Here: 


Zjj= u 

Figure 3.14: Wander Azimuth (NEU) 



For {n} = UEN 



Figure 3.15: Wander Azimuth (UEN) 

Look carefully at the orientation of {c} and {n.}. To understand each element of the 
transformation matrix, write out each component of {c} separately and compare with 
the matrix form in Equation 3.34. 

Xc = —pn sin a -j- z„ cos a 


29 



Vc = Vn COS a + Zn Sin a 
— ^n 


X 


0 —sa ca 


X 

y 

— 

0 CO; sa 


y 

z 

c 

_ 1 0 0 


z 


4. Navigation to Body: \p 

Define {b} & {n} using right hand rule => NED 


( 3 . 34 ) 



Figure 3.16: NED Orientation 

Convention: 

• <j) positive when right wing down. 

• 6 positive when nose up. 

• tl> positive when nose rotates N E. 

Then: 
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X 



Figure 3.17: Positive (j), Positive 0, Positive ^ 

where: 

10 0 cO 0 —s0 cip sip 0 

(7^= 0 c</> , (7^= 0 1 0 , C^= -si^ op 0 (3.35) 

_ 0 —s(p c<p _ s6 Q cO 0 0 1 

I. COORDINATE FRAMES FOR INS MECHANIZATIONS 

Types of INS configurations 

• Local level (torqued) 

• Space Stabilized 

• Strap down 


Local Level 

The x, y accelerometers are always in a plane tangent to local ellipsoid (ideally 
{n}). Torque is applied to vertical axis to maintain certain orientation of x, y axes: 
There are four types of local-level mechanizations: 
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Figure 3.18: Torque Applied to Maintain Orientation 

• North Slaved 

• Unipolar 

• Free Azimuth 

• Wander Azimuth 

In order to maintain a local-level orientation, the accelerometer platform must be 
torqued to align with the axes of the local-level frame. Each of these systems is 
defined by the azimuth torquing rate. 

Question: How to compute the torque? • 

Answer: Must know angular velocity of platform with respect to the inertial frame. 
Let: {n} = ENU 
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Figure 3.19: Geographic and Inertial Torque Model 


Now, 

^ni — ^ne “i" where ■“ angular velocity of tuvt ^3.36^ 

Note: in Siouris [Ref. 1], /9 is used to define fl„e- 
Recall Equation 3.26 


figj — 


■ 0 

Clcosc/) 
_ flsincf) 


(3.37) 


Let: 


be the velocity of {n} wrt {e}. 


V = 


ve 

Vpf 

vu 


(3.38) 


Now, the angular velocity fine can be described as a function of the distance from the 
Earth’s center and the linear velocity u at a point. Therefore, let: 


fine 


UE 

itJU 

UN 


(3.39) 
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Consider Figure 3.20, the angular velocity u)e can be expressed in terms of the 


radial distance and the linear velocity ujv, or: 


E Vw 



Next, from Figures 3.21 and 3.22 and ujj are: 


N Ve 



Figure 3.21: ujj^f 


ve 


(3.40) 


(3.41) 



(3.42) 

(3.43) 
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where 


and 


^ ex 



Figure 3.22: UJu 



e = \ -- = eccentricity factor 


a = semi-major axis and h — semi-minor axis 


(3.44) 


From Figure 3.23 we obtain 

■ _ 

^0,ni = (A + fi) COS<l> 

(A + fi) sin^ 

Let the orientation of the navigation frame {n} be UEN. Therefore, re¬ 
solved in the {n} frame is: 

c4> 0 s<f> cA sA 0 ' ’ 0 1 \ c4> 0 s</> 1 r 0 ' 

“ 0 10 —sA cA 0 0 “1-0 10 —^ 

_ -s^ 0c?iJ[ 0 0 lJ[Ad-fi_ -s(j) 0 _ 0 


(3.46) 


(A + Q,)s(f )' 

0 (-^ + ^)s4> ’ 


_ 0 + 

—^ 

(3.47) 

(A “t- Qi')c(j> 

0 _ . (A -|- Q,)c(j) 



The negative sign in front of the ^ term comes from the orientation of the axes. Since 
the y-axis is east, the upward rotation of <j) is negative using the right-hand-rule. 


(3.45) 
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Figure 3.23: Ct^i 


To verify, point the thumb in the direction of the y-axis and curl the right hand. A 
positive rotation would be down instead of up. Hence, the negative sign. 

Now convert to ENU orientation 

- 

(A + n)c<j) (3.48) 

(A -f _ 

J. PLATFORM MECHANIZATIONS 

Let a determine the drift (wander) angle of {c} = {p} with respect to {n} = {p}, 
the geographic frame and is a function of both the sidereal rate, Ctei and Cine, as shown 
in Equation 3.49. 

0 ca sa 0 1 r 0 

0 = —so; ca 0 0 (3.49) 

. A J L 0 0 1 J [ (A + Cl)s^ J 

Figure 3.24 shows the commanded drift angle a. The rate of change of a or A is 
the difference between the platform angular velocity about the 2 ;-axis and the 
geographic angular velocity or 
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Figure 3.24: Platform Frame {;?} with Angular Rate, a 

In order to keep the platform aligned we must command the angle a at a rate equal 
to uizp, we which we have defined as We can now define each mechanization in 
terms of d. 

North Slaved 

To maintain a north pointing system we must keep pointing North, {NEU} ori¬ 
entation. If we apply torque to keep d = 0 

‘^zc = (A -h Vl)s4> 

Free-Azimuth 

In a Free-Azimuth system, the vertical axis is not torqued, therefore: 

= 0 

Substituting into Equation 3.50, we get 

d = —Uzg = —Qei sin (j) 

The equation at the top of page 46 in [Ref. 1] is incorrect and should be the same as 
Equation 3.50. 
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Unipolar 


In the Unipolar system, the wander angle, a, is kept equal to the longitude. A, of the 
geographic frame or d = ±A. Substituting into Equation 3.50: 

^zc = ‘^zg ± A (sign changes when crossing the equator) 

Wander-Azimuth 

In the Wander-Azimuth system, commanded torque is equal to the vertical com¬ 
ponent of the earth’s sidereal rate, Q.ei sin 4>. Substituting into Equation 3.49: 

d = (lei sin (f> — (lei sin (/> — A sin d 
d = A sin d 

(No singularity at the pole) 

Space Stabilized 

Maintains constant orientation wrt inertial space. 

Strap-down 

Inertial sensors are mounted directly on the vehicle => transformation from sensor to 
inertial axis is computed rather than mechanized. 


K. PLATFORM MISALIGNMENT 

Platform to Accelerometer : “C 

Imperfections in accelerometer installation result in errors between {p} and {a}. 
Assume misalignment angles are small: i.e. sd ^ d? cd ~ 1. Error angles: 

• rotate by d about Xp 

• rotate by 6 about j/p 
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• rotate by about Zp 


(3.61) 

(3.52) 

(3.53) 

(3.54) 

Ignoring higher order terms, we get: 


1 

xp 

-9 1 

0 

xp 

-9 ■ 

-xp 

1 

<P = IsxS + 

-xp 

0 

<P 

9 


1 

J 

9 


0 


' <!>' 

= /3rr3 + 5(A),A = 9 (3.56) 

. V’. 

pC is a linear function of A. In general , for small changes, Euler angles 
act like vectors. These small misalignment angles can be used to compensate for 
misalignment errors. (In this derivation we have assumed {o} to be orthogonal). 

L. ESTIMATE OF PLATFORM TO NAVIGATION TRANSFORMA¬ 
TION: 

Let pC be the estimate: 


C SpC (small error) 

(3.67) 

: {i + syic) y 

(3.58) 
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exp sxp 0 ' ‘ cO 0 —sO 1 r 1 0 O' 

—sip exp 0 0 1 0 - 0 c(p s<p 

0 0 1 s6 0 c9 0 —sp c(p 

'1 V’Oirio-^iri 0 o' 

= -xp I 0 0 10 0 1 

0 0lj[^0 lJ[0-(/>l 

■ 1 Xp 01 \ I 9(p -9' 

= -xP \ 0 0 I <p 

,0 0 I \ [ 9 -(P 1 _ 

1 9<p-{-xp —9-\-xp(p 

— Xp 1 (p 1p(p 
9 -<p l' 



Since ^ = / we obtain: 

(/ + (6^) ;c) f ic (/ +;c(«jc)’') = / 

I + («,"£•) JC + lC(Sff + = / (3.59) 

Since 6C6C^ ^ 0, we get; 

=> (6^)^ = - := M (3.60) 

Post multiplying the left and right sides of Equation 3.60 by gives: 

8^0 = MIC (3.61) 

Adding to both sides and remembering that '^C = —^C for a skew symmetric 
matrix, Equation 3.61 becomes; 

^^^C = {I-Myf (3.62) 
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IV. INERTIAL SENSORS 


A. LASER GYRO FUNDAMENTALS: PASSIVE SAGNAC INTERFER¬ 
OMETER 



Light is split at A to travel cw and ccw. Suppose fi = 0. Then: 

t- — 
c 

Suppose fi 7 ^ 0. Let X be the distance A traveled in inertial space for light to return 
to beam-splitter. Then: 


Then: 


t± = 27ri? ±A'±, whereAj- = Rfiij- 


, 27rR RQt± 

t± = -+- - t± 

c c 


2wR 

Rfl 
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Then; 


N = t+-t.= 
= 2 'kR 


2-kR 
c — RQ, 

2Rn 


2TrR 
c + RQ, 


Lc2 - i?202j 

AirR'^n 


c2 - R^a^ 


1 

AttR RQ, 


C 


RCl 

RQ' 

2 

i +-+ 



c 

. C . 



Now, to a first approximation: 


^ AttOR^ 

Af =-r— <(= Sagnac effect 


The optical path difference: 

AL — cAt = c 


Airm^ 


ArR"^ AACt 


A = 4x7?^ 4= area enclosed by the circular path 
Sagnac effect is used in RLG and FOG. 


B. FIBER OPTIC GYRO 

0 is measured by analyzing the phase shift caused by Sagnac effect 

• Ideal behavior assumes reciprocity the paths of cw and ccw beams are iden¬ 
tical the phase shift is due to inertial distance traveled 

• Non-reciprocity may arise from non-linear index changes caused by unequal 
intensities =» is handled by using broadband source 

• Non-reciprocity due to external magnetic fields like Faraday effect can be re¬ 
duced by magnetic shielding 
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Photo 

Detector 



Figure 4.2: Fiber Optic Gyro [Ref. 1, p. 119] 


• Fiber length: 50m to 1km 
FOG elements: 

• Laser diode that acts as a light source 

• beam splitter 

• coil of optical fiber 


• photo detector 






Light from laser diode is split into two beams cw &: ccw. The beams are super¬ 
imposed and the resulting interference is monitored => measure phase shift by using 
photo detector convert light into electrical current. Current is sampled every 
frame. Let A be the wavelength of the light. Then: 


T 

and phase shift 


A 


27rAi 

~Y~ 

c 

[27r2Tr{27r R)m] 


AttLR 

Ac 


Ac 

n, for = 1, number of turns 


For N > 1, L = 2ttRN and A = ‘kR? 


= 


K = 


SttN A 

Ac 
SttNA 

Ac 


0 


sensor scale factor 


Note, Sagnac effect can be viewed in terms of At or A(f). 
Two practical approaches to obtain 0: 


1. Measure A(l> (=^ At) 

2. Change the frequency of one of the beams until A<f) = 0. 


Then: 



(4.1) 
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V. TANGENT PLANE NAVIGATION 


A. TANGENT PLANE EQUATIONS 

Consider the case of flight in a iimited region. Use tangent plane; 

X = North 
V = East 

Z = Up: Away from the earth’s center 



Figure 5.1: Tangent Plane 

Let ifi be the unit vector in the direction away from the earth’s center. Then 
the local gravity vector g acting on the observer is 































where 


R = Po — Ro and R = |^|, 

Ro — vector connecting E.C. with the origin of the tangent plane. 
Po = position of the observer in the tangent plane. 

Let Po = (a;, y, z) then, 

\\R\\ = ^Jx^ + y^ + {z + RoP 

X 0 X 

R= y - 0 = y 

Z ^ . ~Ro . . d* Po 



Therefore , the local gravity vector is: 



Note, for flights near the Earth’s surface, R ~ Ro, then: 

' 9x ^ 

< 9, - -jrj ( 5 . 1 ) 

, 9z - -9o + 2^2 

Where 


GM 



B. GENERAL NAVIGATION EQUATIONS 

• Idea: Compute vehicle’s position based on accelerometer readout & equations 
derived above. 

• Constraint: Einstein’s Uncertainty Principle Accelerometers cannot distin¬ 
guish between inertial and gravitational forces. 


46 



Therefore: Let 


Then 


Where 


R — geocentric position vector of vehicle 
W = vehicle’s inertial velocity 

\ R = 

[ = A gm{R) ^ Einstein’s principle 


A = specific force measured by accelerations 
gm{R) = gravitational acceleration 


(5.2) 


Equation 5.2 represents a system of differential equations that can be integrated to 
get vehicle’s position in an inertial frame. 

Recall, ir = A + g^ and we get: 

{ Ax — X — gx = £ -j- ^x 

A = i/-gy = g + ^y , (5.3) 

■^Z = Z - g^P^z- 2^z + go 

where 

GMR^-GM{z + Ro)SR^^^^^ 

9o H ^ 



gM{z + Rq) 

R3 


— ” + 


9o H" 


dz 


gM[z + R^) 


[GMR^ - {GM{z + Ro)SR^) 


R^ 


2GMRRo 

go -- 2 ; 




-9o + —z 

ILq 


The mechanization for Aj,, is shown in Figure 5.2. 
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X(0) x(0} 



Figure 5.2: Block Diagram of Inertial Navigation System 
Solve the differential equations in Equation 5.3 for and ^ to get: 



Therefore, for constant thrust acceleration in x and y direction the vehicle is 
displaced to an average position such that the gravitational component in x or y 
direction is equal to imposed thrust, acceleration. The vehicle oscillates about this 
mean position with a period of: 

T — —\l— — 84 min 
27r V go 
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Thrust acceleration in the ^-direction greater than go causes exponentially increasing 
velocity and position. 


C. ERROR ANALYSIS 

Linearize eq. 5.3 at 


Therefore: 


d'x = —^6x + 6Ax 
6z = 2^6z + SAz 

rin 


(5.4) 


8x 

Ro 

6z 

Ro 


6 Ax ! j go 6y 

9o \ \j Ro ) Ro 


Therefore, errors in x and y are sinusoidal, but errors in z increase exponentially 
with tinne. do not use dead reckoning to compute z. 


D. THE VERTICAL CHANNEL 

• Integrating acceleration in z leads to divergent solution 

• Use altimeter measurements together with to obtain altimeter data. 

• Altitude is computed in CADC (Central Air Data Computer) based on 

“ Tfat = free air temperature 
- Ps =static pressure 

Definition’s 

• Absolute altitude {Habs) Height above the earth surface at a given location. 


49 



absolute 

altitude 



Mean Sea Level 

Figure 5.3: Altitude Definitions [Ref. 1, p. 199] 


• True altitude {Htme) Actual height above the standard sea level. 

• Pressure altitude (Hp) Height in model atmosphere above pressure datum plane 
of 29.92 in. (760mm) of mercury. (Does not consider variations in pressure and 
temperature at sea level, unlike true h). 

• System altitude Computer corrected Hp for non-standard day variations. 

ICAO Standard Atmosphere [Ref. 1, p. 201]. 

• Earths atmosphere 

— Troposphere : Lowest layer. Characterized by decrease in atmosphere (neg¬ 
ative lapse rate). Contains most of air and moisture. 

— Stratosphere : Lapse rate changes direction. Marked decrease in water 
vapor. 

- Metosphere : Lapse rate again reverse and T decreases with altitude. 





- Thermosphere : Above 80km, air temperature decreases with altitude. Gas 
molecules are separated by large distances. 

E. ATMOSPHERIC MODEL 


P = pressure 
Z = geometric altitude 
dP — —pgdz 


Where 


9 

9o 

P 


9oRI 
{Re + 

9.87mls^(32.2ft/s^) 
atmospheric density 


From the ideal gas law 


Where 


_ MP 

^~Wf 


M = mean molecular weight of the air 
R* — universal gas constant 

T = absolute temperature 


dP 


gM 

'r*t 


dz 


dlnP = -^dz 
R*T 

P = Po exp —^^z 
^ R*T 


(5.5) 


(5.6) 
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Where Po = pressure at sea level 
Similarly 

p = po exp 


gM 

R*T^ 


Let 


r 


Then for a given layer 


dz 


(lapse rate) 


T 

r 

p 

p 


To-Vz 

dz 



Also , from US atmosphere model, we get: 


T = - Ho) + To 

and from Equations 5.5 and 5.6 

T ^ _ l) 

H, = /f, + LU_pii_h,r#o 

H, = if„-^ln(Pa),r = 0 

Where, To,Ho,Po are obtained from US Standard Atmosphere Model. Numer¬ 
ical examples [Ref. 1, p. 204]. 
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F. VERTICAL CHANNEL DAMPING: COMPLEMENTARY FILTER¬ 


ING 

Idea: Use altimeter information in steady state and at low frequencies (altitude 
is noisy). Use acceleration data at higher frequencies. 

How ? Consider; 
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Figure 5.4: Complementary Filter Mechanization 




















Consider Figure 5.5 (see Figure 4-16 [Ref. 1]). We can write the following: 



Figure 5.5: Complementary Filter: 2 -Channel 


(from CJiDC) 


ii = x^- C2{hs + X2) 

X2 = Xi — Ci{hs X2) 


ii = —C2X2 — C 2 hs Xj 

X2 = — C1X2 -f xi — cihs 


In state space form we have: 


0 -C2 Xi -^2 1 , , [ 1 1 .. 

1 "r hj 4- Xj 

1 —Cl X2 —Cl 0 


h = Xz = 0 1 


In transfer function form, h(s) = C{sl — A)~^B we get: 


h{s) =01 


S C2 
-1 5 -f- Cl 


Taking the inverse of {SI — A): 


sI-A)-^ = [ 0 1 


S C2 
— 1 sA Cl 


h ^ ^ ■■ 

-Cl 0 


1 s -h Cl C2 

-1- CiS -I- C 2 1 5 







Substituting 5.9 into 5.8 and multiplying through gives: 

ks) = ^-^[0 1 + 

s 2 + Ci 5 + C 2 L J \ [ -C 2 - 5 Ci J L ^ J / 


-(^Ci+C2) ^ j 1 

C\S C2 + Ci5 + C2 


Compare with our filter: 


Now add a. 


Cl = a, C2 = h 


Xi = 02 + 2^1x2 - C2{hs + X 2 ) 

X2 — xi - ci{hsX 2 ) 


(5.10) 


ii = (—C 2 + 2a;^)x2 — C2/ii + Oj 

X2 = -C1X2 + - Ci^s 


(5.11) 


... ' x\ —C 2 , 1 

i:2 " ° -C2 + 2a;, ^ 

J L 1 _cj I I- J L J L J 


h = X, = 0 1 


This gives the transfer function h{s): 


k, + - , .^ ., 

5^ + Ci5 + C2 5^ + Ci5 + C2 


Where: 


Substituting: 


a = Cl 


b = C2 - 2a;2 = 6 - 2cc2 


J' , , as + 6 — 20;^ 1 

2 _L I 1 o 2 2 i i u o 2 ' ^ 

+ as + 0 — 2 c<;f s^ + as + 0 — 2 ta* 
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Note, in steady state must equal Qo- Suppose measurement has errors, i.e. let 


^ZO "h ^ZO — 9 o 

and suppose 6azo ^ 0 in steady state. Compute h in steady state. 
Using the Final Value Theorem: 

h(0) = liinsh(5) 

= lim5 ri(s) ^^(s) + lims 112 ( 5 ) 502 ( 6 ) 

Where: 

r.W 

T2(s) 

Suppose: 

Sttz = const 2 => 502(5) = 

5 

, , 7 / \ const j 

hs = consti 0 ^( 5 ) =- 


05 b — 

5^ + 05 + 5 — 2a;2 

_ 1 _ 

5^ + 05 + 5 — 2a;2 


^5(0) - Ti(0)5,(0) + r2(0)5o2(0) 

= hs(0) + ™ 2 ^«^( 0 ) 

^5 


Note, we want ^(0) = ^^(O). 
Consider 


5 ( 5 ) = 


5 ^ + 05^ + 55 + 

5^ + 05^ + 55 + C * 
5^ + 55 + C 
5^ + 05^ + 55 + C 


05 


5^ + 05^ + 55 + C 


7 502(5) 


fi{s) 


T 2 (s 
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Where 
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VI. KINEMATIC EQUATIONS AND ERROR 

ANALYSIS 


A. ROTATING COORDINATES 


Q 


Figure 6.1: Rotating Coordinates 


= ‘Po, + tC'Rg (6.1) 

Suppose: 

^Poj, = 0 and ^Pq = const 

Then: 

% = tC- ^Pq 

Differentiating: 

i'p^ = iP^p« = i(prp^ 
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Note 


= ;cs(p%,) 

= pfCS(%i)fC^ 


= scQ,i);c 


= s{%i)ic^PQ 


‘Opi X Tq 


= ^CJX Pc 


Now , suppose ^Pq ^ const. Let ^ Pq = vector of time derivatives of each element of 


Then: 




In general, Coriolis theorem: 


^ A ^ . 

~A=-A + u:xA 


Now, let’s compute: 


dfi ^ 

= Is ‘-P® + ^ S s X + ‘ 


w X a; X 


Note: 


(ax6) = dx6 + ax6 
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Therefore: 


6t C"') "" “ (a "" ‘^-5 + *u; X ^ Pq 


d ■ 8 ■ '■ 8 ■ 

— ^=—^+^ujx *u; = — 'a; 

dt 8t 8t 


Therefore, we get: 


dt^ ~ 8t^ + 

'cux -*Pq+ *a;x (*a;x %) 

" h'^‘‘^{jt'‘^ ^ ■Po+2Xx^‘P 8+ -i^x (Vx ‘Pq) 

Notation -^x = x 

Now, suppose Pop ^ 0, then 

^2 r 

‘^<3 = %p + ^ ‘^<3 + X pQ + 2 X - pQ + %i X (‘Opi X Pq) 


Using the notation in Siouris [Ref. 1], 


r — R + p 


Then: 


^ ^ ^ + '^pi X p + %i X (‘fipi X p) + 2 Tipi X 
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Where: 





IC'S 


IC'S 



Figure 6.2: Calculation of Velocity and Position 
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p 
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a 

R 

<t> 

Jn 

P„(sin (j)) 

t 

R 


earth’s gravitational constant 
mean equatorial radius of earth 
\Jxl + yl + zl 

latitude (sin ^ 

R 

coefficients of zonal harmonics (constants determined 
from observations of orbit perturbations of artificial 
satellites) 

associated Legendre Polynomial of the 1st kind 
potential of spherical mass 
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The rest account for: 


* Earth is bulged at the equator 

* Flattened at the poles 

* Generally asymmetric 

Note , if symmetry wrt equator is assumed => odd harmonics vanish. 
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Therefore: 


^ = ^A- + 2"f2e0 X + ’^g(E) (6.14) 

Jp 

from pg.l52 

^ Ve Ae 0 —<jJu — 2 Q,e ojj\j + 2^lpj Ve Qx 

— Vn = An - uju + 2Qu 0 -ue - 29,e Vjv + Qy 

. . . -^u _ _ ~^N ~ 2flN loe + 2Q,e 0 Vu Qz 


Vs = 

_o 

il 

t Vn dt 
Jo 

Vu = 

f Vu dt 

Jo 


In {n} for spherical earth: 


0 1 r 0 

. 9 mo{^Y J . 9 mo{l - 

Finally, note: 


A 


Vat 

Rtf, + h 

Ve 

{R\ + h) cos (f) 


Figure 6.3 shows the standard mechanization for Equation 6.14. 
Wander Azimuth Mechanization 



p 
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— Sea + \c(i)sa 
(f)sa. + Xc(f)ca 
6c + Xs(f> 

COL so; 0 ] r 0 
— so CO 0 ficOS' 

0 0 1 n sin ( 

Q,c<j)sa 

VtcepcoL 

rts(p 

ca so 0 1 r Vs ' 

—so CO 0 Viv 

. 0 0 1 J [ Vc; . 


CO so 0 
—so CO 0 
0 0 1 


R<j> + h 

1 Ve 
{Rx + h)c(j) 

o = —A sin (j) (wander azimuth) 
Alternatively, compute A, and o from direction cosines: 


Integrate Equation 6.15 to get ^C. Then, for {n} = ENU: 


(6.15) 


Where 


cac\ — sas(j)s\ 

saccf) 

—cocsX — soLS(f)cX 

—sacX — cas(f)s\ 

cac<f) 

sosA — cas(f>cX 

C(f>s\ 

S(f> 


C(f)c\ 

Ci\ 

Ci 2 

C\z 



C22 

C2Z 



Cz 2 

Czz . 
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And 
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Now wander-azimuth 


= 0 d = —\s(f> 

—(t>ca + \c(j)sa 
^pe = <f>sa + Xc<j)ca 
\s(f) + d = 0 

3. 

^ = M-(^npe + 2^e0 X ^ + g{R) 

Jp 

4. 

Vn 

R<j> d" h 
Ve 

{R\ + h)c6 

—Xsd> (for wander-azimuth). 


<P = 

A = 

d = 
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Governing equation: 


‘CMh- ^g= ;Ci^A+ ^g) 


z 



Figure 6.4: Latitude and Longitude in {e} Frame 


Therefore 


^R = 


{R + h)c(f)cK 
(R + h)c<j)s\. 
(i? T K)s<p 


A = tan ^ ^= A — Ao + fl 


A = Aq + tan 


-1 (% 


«;'+ 


A -A ' 

® = sm T——- 

VR + h^ 




+* f2ei X R 


I 


Recall 








Then 


’^V= = fC(^R- R) 

* Compute ”y; 



C(j) 

0 

s<p 

cA 

5A 

0 ■ 

fC = 

0 

1 

0 

—sK 

cA 

0 


— S(j> 

0 

C(j) 

0 

0 

1 


c^cA c<psA s<j> 
—sA cA 0 
—s<j)cA —s<^sA c(j) 


Finally, = const = initial values 





Figure 6.5: Navigation Mechanization 


Assume ellipsoidal earth 












* Torquing the same 


Governing equation: 


R 

A 

A 

<!> 


= tan 


= sin 


{R\ + h)c<f)c\ 
{R\ + h)c(j)s\ 

[(1 — s^)R\ + h] s(f) 

\^Rx) 

Ap “I* A — fit 


or consider 


Xl-s^)Rx + h^ 


( Rl + = {{R, + hf cos^ 4>) ' = (i?, + h) cos 

tan 6 = —p== 

/-RJ + ‘-R? 

Example Wander Azimuth Navigation 
{n} = t/EtV 

Vehicle is traveling in a plane defined by (f>o = 45‘ with the equitorial plane. Vehicle’s 
velocity V along it’s path is 400^^. Vehicle has wander azimuth system aboard. 

1 . What is 



1 

o 

_1 


1- 

o 

II 

e 

400 

0 

— 

178^ 

sec 

0 


2 . What is ”0^^, "fi„ 


®fi - 



0 1 


0 1 


0 

Z= 

0 


V 

, (Rcos<l>o) - 


A 
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Figure 6.6: Wander Azimuth Navigat 




Now 


n 




C<l>o 0 5^0 

0 1 0 

_ —S<Po 0 C(l)o 

\s(Pq 

0 

tj3;Il (^Q 

0 

v_ 

I R J 


cX 

—sX 

0 


Check Figure 6.7 


sA 0 
cA 0 
0 1 


and solve for 




X 



Figure 6.7: U and N Transformation 
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Platform torquing: 


( <^xc = 0,S(l)o *— wander azimuth 

= -f fis^o)5Q: 


UJ, 


4. What do accelerometers measure? 


'd^R" 


dt‘^ . 

\ / % 

^gm{R) = 


= ^A+^gr,{R), 


— -E- 1 


r ^ 1 

B? 


(Ro+hf 

0 


0 

0 


0 


5. What is in terms of ”V, ^Clne, ^f^ei? 

Recall 


'dR'' 


'dR 


Differentiating: 


dt)^ - 

= V flei ^ R 


=f^) +neiX{V + ^eiXR) 


dt^ 


?^Pplying the Coriolis theorem to . and expanding the cross product terms, 
we get: 

{d^R\ (dV\ 

Y dt^ j ~ j X y + Dei X y 4- Dei X (Dei X R) 

— X y + Dei X y + Dei X (Dei X R) 

— 2Dei) X y + Dei X (Dei X y) 

= 2Dei) X y + Dei X (Dei X y), where Dei » 0 

~ (D,e + 2Dei)xy+|^^j (6.16) 
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D. ERROR ANALYSIS 

* Idea: Perturb the navigation equations around the true values of the position 
and velocity vectors. Consider tangent plane mechanization, Figure 6.8. 



Figure 6.8: Tangent Plane Mechanization 


Let V = r 

Then V = A + g{R) 

( r ^ V 

^17 = g{R) + A 

From Figure 6.9 R= ^R— ^R^c + V 


Therefore we obtain 
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Figure 6.9: Rec 


Now, suppose pC is known exactly but, 

r = To + Sr 
V = Vo + SV 
= A„ + SA 


(6.19) 


Where Tq, K, Ao are true values and ^’s are small errors. Now let’s linearize 6.19 
around To, K, Ao to obtain a linear model of the error dynamics. 


SV = SA+[ ^ ^ ^ ] Sy 

Sz 


Where 


Sr= Sy , SV = SVy 


■ SA^: 
SA = SA, 


Consider 


fix 

ii«r 

u-y 

ll«IP 

ti[z-\-Ro 

l|H|P 


same for^^(i2) 


_§- ij 
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Where 



X — state vector consisting of vehicle position and velocity 
u = accelerometer inputs 

Now one can be used to obtain error equations for INS mechanization. 


Let 


X = Xo + 6x 
u — Uo-r 6u 


where Xo,Uo are true values and 8x^8u are errors. 
Then 


8x 




8u 

0 


A8x + B8u 


( 6 . 22 ) 


Equation 6.22 describes error dynamics of the navigation equation 6.21. Now, let’s 
consider the vertical channel equation in the tangent plane. The mechanization is 
shown in Figure 6.10. 



Figure 6.10: Tangent Mechanization: Vertical Channel 
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then from Equation 6.20 we obtain: 


{ Sz = Svz 
^ Svz = + 8Az 

The mechanization for the vertical channel error equations is shown in Figure 6.11, 



Figure 6.11: Vertical Channel with Error 


_ 1 

where the poles are which show that the errors in the vertical channel blow 

up. Therefore, let’s use a complementary filter to stabilize the errors, Figure 6.12. 



Figure 6.12: Vertical Channel Complementary Filter 


Here 8 represents changes in hg. Complementary filter equations: 
8 x 1 = —a 8 x 1 + 8 x 2 + a 8 hs 

< 8x2 = —{b—^)8xi-{-(^b — ^)8hs-\-8Az8xi-\-8x2-\ra8hs 
, 8 z = 8 x 1 
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—a 1 6xi a 

-i 0 Sx2 + b 


Sz=\l ol[f' 

L J 0X2 


0 

1 


(6.24) 


Question: How do we implement filter 6.24 with the nonlinear Equations 6.23? 
Answer: Replace ^’s with full scale signals in Figure 6.11 and ^ with 



tr 


l/s 

1 

X 2 


7 /« 



JL/ S 


Xi z 


p (Z+Ro) 

■ I . 

II^P 

- ■— - 

h 


Figure 6.13: Linearized Vertical Channel Filter 

Linearize Figure 6.13 at Xo, yo, Zq, A^o to get Equation 6.24. 


—axi A X 2 A ahs 
—bxi + bhg + 

Xi 


(6.25) 


X\ — 2 : 3^0 ^^1^ ^2 :^2o 4” bx2<] big — bgQ 4“ Sbg^ . 


{ ^ 2:1 = —aSxi + 6x2 4 - a6hg 

8x2 — — b8x\ “j- b8hs -f" ^^8x1 8Az 

Note Equation 6.26 is the same as Equation 6.24. 


(6.26) 
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Summary: Filter design process 


1. Obtain INS mechanization equations. 

2. Linearize around true values of INS states and inputs. 

3. Design a filter for the linear equations obtained in step 2. 

4. Implement the filter on the non-linear equations in step 1. 

Probability and Stochastic Processes 
The errors in accelerometers, altimeters, GPS, gyros, ... are usually modeled 
as random signals having certain statistical (stochastic properties) properties. This 
is shown in Figure 6.14. 



Figure 6.14: Random Process 

Definitions 

• Freeze time: x{ti) is a random number with values in some interval of R (real 
line). 

• F(a:(t)) = average (mean) value of x over time. 

• F[(x(t) — x(t))^] = variance of x{t) {rms^). 
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T is called covariance of x{t) and is denoted 
by Px{i,'i')- Note: variance of x(t) = 


• Given x{t) and y{t): two random processes, we define 

P„ = E{{x(t) - i(t)){y(T) - y(T))] 

as the cross-covariance of x{t) and y{t). 

If x{t) is a vector of random processes: 

Xi{t) 

x{t) = : 

Xn{i^ 

■ E(x,(t)) - 

E(x(t)) = x(t) = j 

_ E(Xn(t)) _ 

Px{t,T) = E[{x(t) - x{t)){x(T) - «(t))^] <— is a matrix 

(T a;i(t) - xi(t) 1 \ 

= E : a;i('r) - «i(r) ••• a;„(r)-i„(r) 

V _ ^n(t) ^n(t) _ / 

■ £'[(a:i(t)-^i(t))(a;i(r)-®i(r))] ••• £;[(a;i(t) - fa(t))(xn(r) - x„(r))] ‘ 
. E[{xn{t) - x„(f))(a:i(r) - xi(r))] • • ■ E[{xn{t) - Xn{t)){xn{r) - ^nC^))] . 

P is a n X n symmetric positive definite matrix with variances on the diagonal and 
cross-covariances off the diagonal. 

Definition : a:(t) is called wide sense stationary (wws) if x{t) = constant and 

p(t,T) = £[(x(t) - - s(T)n 

= P{t-r) 
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From now on we will deal with E{-) which is a linear operator; 

E(xi+X2) = E{x2 + Xi) 

£( const) = const 

E{Mx) — ME{x), where M is any constant: scalar, matrix 
E{xM) = E{x) ■ M 

E[{x{t) - x)(x(t) - f )^] = E[x(t)x(r) - x(t)x^ - xx(t)^ + xx'^)] 

= E[x{t)x{T) - E{x{t))x'^ - xE{x{t))'^ + xi^)] 

= E{x{t)x{T)) — xx^ — xx'^ + xx'^ 

= E{x(t)x(T)) - xx^ 

If t/ = Mx, a: & : y are wss processes and M G - constant matrix. 

Then 

y = E{Mx) = ME{x) = Mx 
Ry = M Rx Rx = covariance of x 

Those are important identities. Suppose x{ti) is independent of x{t 2 ), x{t 3 ), • • •, x{tn) 

for all ti, • • • ,t„. 

Then 

E[{x{ti) - x){x{t 2 ) - xY] = E[x{ti)x{t 2 )] - xx'^ 

= E{'x{ti))E{x{t2)) — xx'^ 

= 0, Vti,t2 

Note, for independent random wss process x(t), 

■ F[Mt)-xi(t))(a;i(T)-Xa(r))] - xi(t))(x„(r) - ^,(r))] 

Rx{i,T) = ■ ■ 

E[{xn{t)-XnmxriT)-xiiT))] £[(x„(t) - x,(t))(x,(r) - f„(r))] 

R, t = T 

0, t/0 

= R6{t - t), is symmetric, positive semi-definite matrix 
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Definition : x{t) is white gaussian noise if it is independent and for each fixed 
time ti, x{ti) has gaussian distribution f(x{ti)), Figure 6.15. 



Here x is a scalar random process. 








in the matrix case: 




{2-KYdet Pj 


exp - xfP^ 



If x{t) is white gaussian we write: 


X ~ N{x, Px) 


Finally, consider 

Then 

& 


is a solution of: 


X = Ax -V Bw, w ~ N{0,R^) 

f(t) = E{x{t)) = 

Px{t) = E[{x{t) - x{t))(x(t) - x(f))^] 
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P^t) = A P^t) + P^t) A^ + BP^ 

If A has all A’s in left half plane, then Px{oo) = P^ (steady state covariance) 
0 = A Px + Px A^ + B Ryj B^ (Lyapunov Equation) 
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VII. NAVIGATION USING NAVAIDS 


Purpose : Compute vehicle’s position and velocity using navaids and determine 
how accurate these computations are. 

Let X e R"' he the vehicle’s position, (n = 3). Let y e he the vector of 
navaid measurements p < n = Z. In general (by geometry, trigonometry, ...) we have 


y = 9{x) 

Let Xo, po be nominal quantities oi x then 


= Xo + 8x,y = y^-\- 8y 


h 


8y = 


dg^ 


dx 


8x 


Equations 7.1 and 7.2 represent p equations with n unknowns. 
E 2 g: Planar navigation using bearing fixes. Figure 7 . 1 . 


(7.1) 


(7.2) 


y\ = arctan 
j /2 = arctan 




xi-bi 


y = gix) 


(7.3) 


Measure yi, y 2 '. assume (oi, 02 ), ( 61 , 62 ) are known and solve for xx, x^. In 
Equation 7.3, yi is called a fix. The nature of the surface y, = const determines 
the nature of the fix: linear, conic, circular, hyperbolic, etc... It is difficult and time 
consuming to solve y = g{x) directly. We need to come up with an interative method: 
Let p = n and 


y = yo + 8y 

X = Xo-¥ 8x 
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Beacon 1 



Beacon 2 

0 



Vehicle (Xi,X2) 


Figure 7.1: Two Bearing Fix 

6x — C~^5y (if inverse exists) 
The estimation mechanization is shown in Figure 7.2. 



Figure 7.2: Error Estimation 


= {ty'' Jyi = c-^Sy, 

< r. /V 

Xi — X{^'y “{” Sxi 

, = yi- 9{xi) 


Estimation Newton Algorithm 
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Error Analysis 

Usually y is corrupted by noise 

y = g{x) V <— vector of measurement noise, 


Assume v is small and Uq = 0 : v — 6v 


Then 

6y = 


and the estimate error is: 



T 

8x V = C 6x + V 

0 


Cx = Sx — 8x = Sx — C ^ 6y 
= 6x-C-\C 6x + v) 

= 6x — 6x — C~^ V 

= „ 

Let V ~ N{0^Ry). Then 

and 

Re. = C-^ Rv C-^ = {C^ R:^ C)-^ (7.4) 

This formula allows us to determine the best combination of fixes: “i2ea: should 
not be much greater than Ry.” 

Rule of thumb : Have the fix surfaces as orthogonal as possible, Figure 7.3. => 
Columns of C will become orthonormal. The less orthogonal the more dependent the 
columns of C are, the greater the noise amplification in 7.4. 
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Note: 


d , 1 

— tan u =-- 

du 1 + 


dy\ 

X2 — <12 

dxi 

(a:i - aiY + (x2 - 02)^ 

dyi _ 

Xi — CLi 

dx2 

(xi — aiY + {x2 — ^2)^ 

to 

1 

^2 — b2 

dxi 

{xi — biY + (0:2 — 62)^ 

dy2 _ 

xi - bi 

dx2 

(xi — biY + (x2 — ^2)^ 


^ r ^ d^ix. 

_ dxi dx2 

. dxi dx2 


e .T = C ^ 


= C-' i?, = (C^ i?;' C)-^ 

Suppose p > n redundant measurenaents. 

Given 

y=g{x) + v (7.5) 

Question : Do redundant measurements help? 

First, how can we use redundant measurements? Suppose we know fv{v) = 
probability density function of v. Note, the values of v for which fv{v) is large are 
more likely to occur than those for which fv{v) is small: 

fv{v = a)dv ~ P[a < u < a + dv] {dv is small) 

From Equation 7.5 

V = y-g{x) 

=> find X which makes fv{v) = fv{y — gi^)) large: 

maXxL[x) = maxxfv{y — g{x)) 
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Here L{x) is called the likelihood function maximum likelihood method. 
In practice we assume 

V ~ iV(0, Ry) 

=» fv{v) = ~J===1= exp v'^ R-^ V 
yj (27r)P det Ry ^ ^ J 

It turns out we only need to maximize 


^ T 0-1 

exp --V V 


Li{x) = I [{y -9i^)f Rv^ {y-9{x)) 


dLi{x) 


= 0 : n equations and n unknowns 


Recall, in the neighborhood of nominal position 

6y = Sx = C 6x 

^ ' 0 


Consider 


Li{Sx) = -{6y - C 6xf R-^ {6y - C Sx) 


dLi(Sx) 

dSx 


d ri 
dSx .2 


{Sy^ R-^Sy - 6x^ C R'^ 6y - 6y^ R;^ C Sx + Sx'^C^R^^CSx) 


-{-2CR:^Sy + C^R-^CSx) = 0 


:^Sx = [C^R^^C] ^C^R:^Sy 


Compare equation 7.6 with equation (6.47) in the text. For Ry = I this is the so 
called “least squares solution”. 

Example : 1-D navigation. Suppose we measure position x with two errors. 


f yi 

= 

a; + ui 

i 92 

= 

X + V2 

■ 

1 ■ 

■ 

=>y^ 

1 

X + 
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Assume 


V = 


V2 


N 


1 0 
0 3 


i.e. j /2 is 3 times less reliable than y-i. 
maXxLi{x) ■ 


1 

1 

1 

H 

T 

' 1 

0 ■ 

-1 r 

2 

1 

(M 


0 

3 



y\ 

y2 


(yi - + ^(?/2 - 


dLijx) 

dx 


■{yi -x)- i (?/2 - a;) = 0 


X = 


32/1 + 2/2 


weighted sum 


Where more reliable measurements are given more weight. 
Note 


X = 


[c^r:^c]~" c^R;^y 

(\l 1 


1 0 
0 ^ 


n\ -1 


4 3^^ ' 


3 j 

32/1 + 2/2 


1 1 


1 0 
0 


Error Analysis (p > n) 


Cx = Sx — 6x 

= 6x-[C^R:^c]~^C^R,6y 
= Sx - [C^R;^c]~^ C^Rx iCSx + v) 

= -[c'^r:^c]~'c^r:\ 

ex = [C^R:^C]~' C^R-^v = Mv 
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= MR,M'^ =[c'^R-^C] ^ C^R-^R,R;^C[C'^RZ^C] ^ = [C^R-^C] ' 

Example 

B - ^]-r 

iiv [ 0 3 J ’ ^ 

r ^ - T 

M = 

^ex — 


3 1 

4 4 


A A - ^ 

16 16 “ 4 


ri 01 


■ 3 ■ 
4 

1 

CO 

o 

_I 


1 

- 4 . 


Compare this with noise for yi : R^i = 1 & ?/ 2 : Rv 2 = 3. 


We do better by using both signals. 

Use Maximum Likelihood : The mechanization is shown in Figure 7.5. 


Xo 



Figure 7.5: Maximum Likelihood Mechanization 

^ ~ (if) |. • ^yi — yi~yi'i 

^ Sxi = [c^R-^'c]~'C^R-Hyi- 

Xi = x~^ + 6xi 

\ Syi = 9{xi) 


Error Model for Wander Azimuth Navigation 

^R — vehicle position in {e} 
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Then 


Now, let 




+ "flei X ‘R 
;CPV+ X 

M + ^g{ ^R) - (J’Qpi + X W 

- ( "Qpe + 2 Pfiei) X ^’y + M + ^g{ ^R) 


(7.7) 


py = py + = py^ + sv 

^R = PR, + P6R = ^R, + 8R 

PA = PA, + PSA = PA, + SA 

Note, given a, b, c, € R^ and c = a x b, then 

dc _ d{a xb) d{-b x a) dS{-b)a ^ 
da ~ ^ “ da ^ 

and 

dc _ d{a X b) _ dS{a)b _ ^ 

m ~ db ~ ~~W~ ~ 

Therefore, we get 

dR = PCSV + S{^n,i)6R 

dv = -S{pnp, + 2Pn,i)6V + 6A+^^§i^SR ^ 

Note, in equation 7.8 ( ) was used for differentiation. This is abuse of notation. 
However, since we integrating both sides which are resolved in correct coordinate 
systems, it is OK. 

Recall, 

The vector in is shown in Figure 7.6. 
where 

’in=‘Ak /i=|rJJ|| 
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Figure 7.6: Unit Vector, in 


JL. 

'R^ 



O 

\ _ 

1 

o 

t 


o 

_ 1 


1 - 

1 

1 _ 

II 

-1 

0 

0 


At 

RA 


0 


1 

o 

0 

f_a 

l___ 


1 

O 

_ 1 


- 1 

o 

1 


Therefore, we get 


d^R 


Finally, we obtain 


dPC^gi^R) 

d^R 


IC 


IC 


d^g{ ^R) 


d^R 

— 2°. 
Ro 

0 

0 


0 

_ £o_ 

Ro 

0 


0 ■ 
0 

_^ 

Ro J 


A. 

dt 


■ 8R ' 


8V _ 



si^nei) 


ic 

-S'(POpe+2J’a0 


8R 

8V 


+ 


I 

0 


8A 


We should be able to verify this model in Simulink. 
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VIII. THE NAVSTAR GPS 


The NAVi gation Satellite Timing And Ranging Global Positioning ^stem is a 
satellite-based radio navigation system with the capability to provide locating data 
to an unlimited number of users T The first satellite was deployed in 1978 although 
the first receiver did not become commercially available until 1982 [Ref. 10, section 
1.2]. This system is the product of experience gained from several previous space- 
based navigation systems like TRANSIT AND USAF System 621B. It is comprised 
of three segments: 

• Space segment 

• User segment 

• Control segment 

Each segment is discussed in the following sections. 

A. SPACE SEGMENT 

A total of 24 satellites now constitute a fully operational space segment. Twenty- 
one of theses space vehicles (shown in Figure 8.1 operate continuously, while the re¬ 
maining three act as orbiting spares. Today, only three Block I satellites remain in 
orbit. These satellites were the first GPS satellites in space. They were launched from 
1978 through 1985 [Ref. 10, section 1.2]. Block I space vehicles weigh 960 pounds 
and generate 420 watts of electrical power [Ref. 8, p. 27]. The remaining satellites in 
orbit were creatively named the Block II vehicles. These vehicles are less vulnerable 

to radiation and have more memory. With this increase in capability has come an 
^This chapter is an abridgement of Marquis [Ref. 5, Chap. 2] and is included for continuity. 
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increase in weight and power requirements. These newer satellites weigh 2000 pounds 
and generate 700 watts of electrical power. 


The space vehicles are inserted into orbits defined by the six Keplerian constants; 

• semi-major axis, a 

• orbital eccentricity, e 

• orbital inclination, i 

• ascending node, fl 

• argument of perigee, u> 

• time of perigee passage, T 



Figure 8.1: A NAVSTAR GPS Satellite from [Ref. 5, p, 4.01] 
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The semi-major axis of a GPS satellite orbit is nominally 26,560 kilometers. It 
is half of the length of the ellipse which defines the space vehicle’s path. Second, the 
eccentricity (different from the eccentricity defined with respect to the shape of the 
WGS-84 ellipsoid) or oblateness of the satellite’s orbit is defined as follows: 


rg - Tp 

T-a + Tp ’ 


( 8 . 1 ) 


where Va is the apogee radius and Vp is the perigee radius. For GPS satellites, the 
eccentricity cannot exceed two percent. The third of the Keplerian elements is orbital 
inclination. It is the angle between the plane defined by the orbit and the equator. 
For example, satellites in a polar orbit have a 90° orbital inclination; those in an 
equatorial orbit have 0° orbital inclination. Block I NAVSTAR GPS vehicle orbits are 
inclined at 63° while Block II satellites are inclined at 55°. The ascending node is the 
satellite’s geodetic longitude as it passes through 0° of latitude toward the northern 
hemisphere. GPS satellites orbit in six different planes. Thus, there are exactly six 
different ascending nodes. The last two Keplerian elements are the argument of the 
perigee and the time of perigee passage. The argument of the perigee is the angle 
in the orbital plane between the ascending node and the closest point of approach 
of the satellite to earth. The time when the vehicle reaches this point is the time of 
perigee passage. The ranges of these last two parameters span all possible values for 
GPS satellites. That is, satellites reach their perigee at all different times of day and 
different locations. 

Ideally, the six Keplerian elements would be sufficient to define any satellite’s 
three-dimensional position and velocity vectors for all time. However, the orbits 
become perturbed by lunar and solar gravity and the earth’s equatorial bulge, as 
well as several other less significant effects. Thus, the number of quantities required 
to fully specify the position and velocity of a satellite in a real orbit is increased 
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to 16. The extra elements consist of time rates of change of the ascending node 
and inclination, i. e. Cl and i, as well as six other coefficients which account for 
variation in the earth’s gravitational field. These 16 coefficients must become part of 
the navigation message. 

The navigation message is the framework in which the GPS satellites broadcast 
their data. One “frame”, the complete message, consists of five subframes of 300 
bits each. Subframe one contains coefficients used to correct the satellite’s clock to 
exact GPS time. The 16 pieces of ephemeris data are broadcast in the second and 
third subframes. Subframe four contains special messages, ionospheric correction 
coefficients, and coefficients for conversion of GPS time to Universal Coordinated 
Time. Ephemeris and health data for the entire GPS constellation is transmitted in 
subframe five. Because of the volume of data in subframes four and five, both must 
be subdivided into 25 pages. Therefore, it takes 25 full frames, broadcast at the rate 
of 30 seconds per frame, or 750 seconds (12.5 minutes) to receive the entire navigation 
message. Critical navigation data - ephemerides, and clock correction coefficients — 
are updated every frame. Secondary data transmitted in subframes four and five is 
provided primarily to assist the receiver in acquiring other satellites. These data are 
not intended to be precise so the lower update rate of once every 12.5 minutes is 
satisfactory. 

Satellites use a pseudorandom binary code superimposed on the two carrier 
frequencies to communicate. The two frequencies are Ll (1575.42 MHz) and L2 
(1227.6 MHz). The Ll frequency carries both the C/A- (coarse acquisition) and 
P-(precise) codes, while L2 carries only the P-code. The less precise C/A-code is 
broadcast at a rate of 1.023 million bits per second (Mbps) and is 1023 bits long. 
Therefore, this code repeats itself every millisecond. The C/A-code is unique to each 
satellite and does not change, allowing GPS receivers to quickly distinguish between 
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space vehicles, even without access to the P—code. The P—code, being more precise, 
is transmitted at 10.23 Mbps with a code length of approximately six trillion bits. 
This code takes 37 weeks to repeat. Since the codes are reset every week at midnight 
between Saturday and Sunday, there are sufficient “code weeks” available in the P- 
code such that one can be assigned to each space vehicle each week. Therefore, 
GPS receivers can easily distinguish satellites from each other with their individual, 
weekly-assigned P-codes. The remaining code weeks are available for uplink from 
the control segment to the satellites. 

B. USER SEGMENT 

The many thousands of GPS receivers constitute the user segment. The re¬ 
ceiver’s functions are to receive and interpret the navigation message and to calculate 
and output position. GPS receivers must determine the time the navigation message 
takes to travel the distance from the satellite to the receiver. This is achieved by 
autocorrelating the pseudorandom binary pulse train received from the satellite with 
the one in memory. A typical civilian GPS receiver must have the C/A-codes for all 
24 satellites in its memory (requiring only three kilobytes). As the pseudorandom 
code is received, the receiver slews its code until the result of the autocorrelation 
function jumps to one. The receiver is now “locked-on” to that code. Multiplying 
the length of time that the receiver must slew its code to achieve a unity correlation 
by the speed of light yields the “pseudorange”. This is not the actual range because 
the offset of the receiver clock is uncertain. When the receiver can lock-on to four 
satellites and thus measure pseudoranges to each, is three-dimensional position and 
clock error can be solved for by inverting this set of four equations 

Pi ~ Xrcvr)^ + {Usati ~ yrcvr)"^ + {Zgati ~ ^rcvr)^ + cAt 

P2 \/(2^50<2 X>pcvr y + {ysat2 — Vrcvr y + {Zsat2 ^rcvT y + cAt 
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y satz ^Tcvr) ”1” (jjsatz J/rcvr)^ “1“ {^satz H“ C^t 


P4 \/ {^sati Xrcvr)^ + {ysaU Vtcvt)^ + ( 2^^044 •^rcur)^ + cAi, (S-2) 

where pi are pseudoranges, [ar^at;, J/sat,,-^sai.] are the ECEF coordinates of a satellite, 
[xrcvr,yrcvT-.Zrcvr\ are the ECEF coordinates of the GPS receiver, c is the speed of 
light, and At is the receiver’s clock error. Having solved this set of equations, the 
receiver now need only transform the solution to the geodetic system and display the 
results. 

C. CONTROL SEGMENT 

The GPS Control segment is responsible for generating and uplinking clock 
correction coefficients and ephemeris corrections for all satellites in the constella¬ 
tion. Five control stations — Hawaii, Ascension Island, Diego Garcia, Kwajalein, 
and Colorado Springs, Colorado — comprise this segment. These control stations are 
essentially GPS receivers capable of constantly tracking all satellites in view. Ad¬ 
ditional capabilities include highly accurate Cesium clocks and recording facilities. 
The first four of these stations track all satellites in view and record pseudorange 
information continuously. This data is sent to the Master Control Station at Col¬ 
orado Springs where it is processed. When all four monitor stations have locked-on 
to a single space vehicle simultaneously, the exact inverse of the standard naviga¬ 
tion problem mentioned in the previous section exists. Since the exact locations of 
the monitor stations are known and their clocks are extremely accurate, the four 
unknowns become the three coordinates of the space vehicle position and its clock 
offset. The Master Control Station calculates these quantities and from them, derives 
the necessary ephemeris and clock corrections. This information is uplinked to each 
space vehicle at least daily. 
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D. DIFFERENTIAL GPS 

Although GPS alone provides highly accurate positioning, it can be made still 
more accurate by augmenting it with a differential station. A differential station 
is merely another GPS receiver whose exact location is known. When this second 
receiver is near the first receiver, both are subject to nearly the same errors, i. e. the 
same local atmospheric properties, nearly identical elevation angles and propagation 
paths to any given GPS satellite, the same clock errors and the same ephemeris 
errors for each satellite. By employing the Pythagorean theorem on its position and 
the satellite position broadcast in the navigation message, the differential station 
can calculate the exact range to that satellite. Meanwhile, it can also calculate 
pseudorange in the standard way (see Section B.). By comparing these two values 
for each satellite in view, the differential station can evaluate the pseudorange error 
to each satellite. These values can be broadcast periodically to be used by receivers 
in the local area to improve accuracy. By using the differential corrections, GPS 
accuracy, even for single frequency C/A-code users, can be improved to one to seven 
meters rms [Ref. 8, p. 64]. 

E. GPS ERROR SOURCES 

Despite its exceptional accuracy, GPS is subject to numerous error sources. 
Clearly, the major error sources must be included in the DGPS model. Error sources 
are: 

• atmospheric delays 

• Selective Availability 

• clock differences 

• ephemeris error 
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• multipath 


• receiver noise 

• Dilution of Precision (DOP) 

Each of these error sources is discussed in detail in the following sections. 

1. Atmospheric Delays 

a. Ionospheric Delays 

The ionosphere is a layer of charged particles between 100 and 1000 
kilometers above the earth’s surface. These particles interact with the transmitted 
GPS signal and slow it, increasing pseudoranges. The equation describing this delay 
is 

40 “1 

= ( 8 - 3 ) 

where At is the delay in seconds, c is the speed of light (3 x 10* m/s), / is the 
system frequency (1575.42 MHz for LI), and TEC is the Total Electron Content 
(electrons/m^) along the signal’s path. The TEC is strongly elfected by the solar 
cycle, season, time of day, and latitude. TEC maxima occur: 

• daily: 1400 local 

• seasonally: spring equinox 

• solar cycle: every 11 years (next 2001-2002) 

• geographic: 20° magnetic latitude 

It varies ± 25% rms at all latitudes during daylight [Ref. 10, section 2.5, p. 24]. The 
pseudorange error due to ionospheric effects can be as great as 40 meters [Ref. 10, 
section 2.5, p. 13]. 
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Figure 8.2: Diurnal Ionospheric Delay from [Ref. 6, p.208] 

The algorithm described in [Ref. 12] which removes 55-60% of the 
ionospheric delay is based on Figure 8.2, which shows the typical diurnal variation of 
the ionospheric delay. The “ACTUAL DATA” curve shown in Figure 8,2 is modeled 
with the “COSINE MODEL” curve, a half-cosine. The equation of the model curve 
is 

At = DC + A cos (8.4) 

where DC, A, Tp, and P (constant offset, amplitude, phase, and period, respectively) 
describe the diurnal variation of the ionospheric delay. DC and Tp are assumed 
constant at five nanoseconds and 1400 local time, respectively. Amplitude and period 
are each modeled as four term power series as follows: 

^4 = Ea.* 

n=0 

P = (8.5) 

n=0 

where the q„’s and are constants which are broadcast in the GPS navigation 
message, chosen based on the day of the year and average solar flux over the past five 
days, and (j)m is the geomagnetic latitude of the ionospheric subpoint. The ionospheric 
subpoint is the intersection of the line between the space vehicle and the receiver with 
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the surface at the mean height of the ionosphere. 


Next, one must first find the subtended earth angle between the user 
and the satellite, EA (degrees) 

445 

^^=dT20-^’ (»■«) 

where el is the elevation angle of the satellite with respect to the user in degrees. 
Knowing EA, the geodetic location of the ionospheric subpoint can be approximated 


<f>I = 4>rcvr + EA cos UZ 

\ ^ , EAsinaz 

AJ — '^rCVT i 5 

COS (pj 


where <j) and A denote geodetic latitudes and longitudes, respectively and az is the 
azimuth of the satellite with respect to the receiver. Now the geodetic latitude can 
be converted to geomagnetic latitude (the required quantity) with the following ap¬ 


proximation: 


<j>m = + 11.6cos(A/ — 291), 


where all angles are in degrees. 


The dimensionless scale factor (SF) which scales the entire delay is 

■ (8.9) 

The final expression for At is a three term Taylor series expansion of 


Equation 8.4 is 


5F. pc+ 4(1-1 + ^)1 for |x|<| 
SF • [DC] for I a; |> f 


for I a; |> I 


where 


27r(t - Tp) 


( 8 . 10 ) 


Due to the 25% rms variation, the error is modeled with a 25% stan¬ 
dard deviation. Therefore, the random part of the error remains within ±25% of 
nominal 68% of the time. 

b. Tropospheric Delays 

The lower section of the atmosphere also causes signal propagation 
delays. Typical tropospheric delay is approximately two meters for 90° satellite ele¬ 
vation (directly overhead) up to 28 meters at a five degree elevation angle [Ref. 13, 
p. 218]. In this application, the atmosphere can be modeled as being composed of 
“wet air” and “dry air”. Dry air is responsible for 90% of the total tropospheric de¬ 
lay, whereas, wet air is responsible for only ten percent. While the moisture content 
in the troposphere is virtually impossible to model accurately, this inaccuracy has 
minimal impact. Numerous models which calculate the tropospheric delay have been 
developed. Black developed the following model in [Ref. 14]. Let 


As 

= 


where 



Asd 


L j, 

Asu, 

= 

J 

II 

l{h,E) 


j. cos E 

^ \l + {l-lc)- 

hd 

= 

148.98(T-4.12) m. 

h-jjj 


13,000 m. 

h 


0.2, 

rs 


6378137 m. 

P 


1 atm. 

T 

=z 

15° C, 


]-I{h = hd,E), 


Ts 


( 8 . 11 ) 
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and As is the wet or dry delay in meters, is the distance from the center of the earth 
to the station, P is the surface pressure in atmospheres, E is the satellite elevation 
angle, and T is the surface temperature in degrees Celsius. It should be noted that Ic 
is an empirical constant. The value of 0.85 is only valid for elevation angles above five 
degree (GPS receivers typically ignore satellites at lesser elevation angles). Similarly, 
kyj is an empirical constant which varies based on latitude and season. The value 
0.20 corresponds to the value for spring or fall in mid-latitudes. This model has been 
shown to be virtually exact at elevation angles greater than 40°, with its worst error 
of about 0.045 m occurring between five and ten degrees of elevation. 

This tropospheric model is assumed to vary 15% from the nominal 
value. Therefore, it is modeled with a 7.5% nominal standard deviation. This main¬ 
tains the random part of the error within 15% of the model value 95% of the time. 

2. Selective Availability 

Selective Availability is a method that the Department of Defense can use 
to intentionally degrade the accuracy of pseudorange measurements. Typically, this is 
accomplished by dithering the space vehicle clock signal. Dithering the clock involves 
encoding the binary time signal the space vehicle broadcasts. The decryption process 
is classified and available only to DOD authorized users. 

The use of SA essentially results in the satellites’ “lying” to the receiver 
about their position. Clearly, this adversely impacts precision. Currently, SA is in 
operation on all Block II space vehicles which comprise the majority of the constel¬ 
lation. The DOD’s stated goal for the positioning accuracy under SA is 100 meters 
(twice rms) for a two-dimensional fix [Ref. 15]. According to [Ref. 16, p. 420], 
the selective availability error can be modeled as a zero-mean, five meter standard 
deviation low frequency noise. The suggested cutoff frequency for SA noise is -^Hz. 
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3. Clock Differences 


The clock model used in this treatment of DGPS is a two state model 
shown in Figure 8.3. It is reasonable to expect the clock to have both a bias and 



Figure 8.3: Receiver Clock Model 


drift. From daily exposure to clocks, the average person realizes that most clocks 
are slightly offset from correct time (bias), and that their accuracy tends to degrade 
with time (drift). From an engineering standpoint, these two phenomena can be best 
modeled with zero mean, white, Gaussian noise. Rewriting the model in state space 
form for further analysis yields 



The'covariance of the clock error state (a:i) can be found by solving the Lyapunov 
equation which can be found numerous control textbooks, one of which is [Ref. 17, p. 
104]. This equation must be solved over a finite time interval (At) because the two 
state clock model is unstable. This interval would normally be the sampling time, if 
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the model were discrete. The result is 


E(x^) = SiAt + 


(8.13) 


By taking the square root of the variance and dividing by At, one finds the more 
standard clock parameter the Allan variance, Va 


Va = 



(8.14) 


A representative plot of the two state model Allan variance cis a function of averaging 
time (i. e. At) is shown in Figure 8.4. 



Figure 8.4: Ideal Allan Variance 

Real clocks behave somewhat differently from this simple model. A typical 
Allan variance plot for a real clock is shown in Figure 8.5. 
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Figure 8.5: Real Allan Variance 

The flat portion of the curve is called the flicker floor. It is the result of 
a non-linear effect which cannot be naodeled by the two state model. This causes a 
significant discrepancy between this simple model and the real world. 

In order for the model to better represent reality, it must be carefully 
crafted to fit the actual plot as much as possible. By carefully choosing the values of 
Si and S 2 , it is possible to make the actual and model curves fairly close. The key 
Allan variance parameters between 0.1 and ten seconds of averaging time are ho, h-i, 
and /i _2 [Ref. 18]. Values of these three parameters for three common GPS timing 
standards are shown in the Table 8.1 from [Ref. 16, p. 428]. 

Brown [Ref. 16] finds that one must choose where the 2-state model is 
accurate. Since normal averaging times are in the 0.1 to ten second interval already 
mentioned, this is the region where the model is made accurate. Maximizing the 
accuracy of the model in that region dictates the following values for the noise spectral 
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TABLE 8.1: ALLAN VARIANCE PARAMETERS FOR THREE COM¬ 
MON TIMING STANDARDS 


timing standard 

ho 

h-X 

h-2 

crystal 

2 X 10-^® 

7 X 10-21 

2 X 10-2° 

ovenized crystal 

8 X 10-2° 

2 X 10-21 

4 X 10-23 

Rubidium 

2 X 10-2° 

7 X 10-24 

4 X 10-2° 


densities: 

ho 

- 

2 

~ (8.15) 

In order to remain conservative in this generic model of DGPS, the least 
accurate clock — the crystal clock — is used. The values for 5*1 and S 2 for this clock 
are 

5x = 4 X 10"^® 

^2 = 1.58 X 10“^^ s-l (8.16) 

These values are be used in the DGPS error model. 

4. Ephemeris Error 

In converting the pseudoranges of at least four satellites (six in this model) 
to a three-dimensional position and clock error, one must solve a series of non-linear, 
coupled algebraic equations. In these equations, the positions of the satellites are 
critical. The only way a GPS receiver or navigation filter knows the space vehicle 
positions is the navigation message. If broadcast satellite positions are incorrect, 
the accuracy of the resulting receiver position suffers. The control segment of the 
GPS system maintains positions on the entire constellation quite accurately.However, 
it would be folly to expect the satellites to broadcast inerrantly accurate positions. 
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Typical ephemeris inaccuracies according to [Ref. 19] are shown in Figure 8.6. 



These errors are resolved in a coordinate system local to each space vehi¬ 
cle. The three mutually perpendicular directions are radial, along track, and cross 
track. Because the DGPS model developed in this thesis does not account for satel¬ 
lite motion, along track and cross track directions cannot be resolved. To remain 
conservative, these two errors are combined into a circular error in the plane they 
define. This error is modeled as zero mean, white, Gaussian noise with a standard 
deviation of 3.5 meters. Likewise, the radial error is modeled as zero mean, white 
Gaussian noise with a 0.5 meter standard deviation. Both of these errors make the 
stated accuracy the two rms point. In other words, the value stays within the stated 
accuracy 95% of the time. 

Since the space vehicle location enters the model in earth centered, earth 
fixed Cartesian coordinates while the error is added in geodetic coordinates, a trans¬ 
formation is between the two coordinate systems must be performed. Having con¬ 
verted the satellite positions to geodetic coordinates, the random errors can be added. 
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However, since the geodetic coordinates contain angles, the random errors in position 
must be converted to equivalent angles in latitude and longitude by dividing by the 
radius of the space vehicles’ orbits, ~26,560,000 meters. The position and the error 
are now in compatible coordinates and can thus be summed. 

5. Multipath 

The signal radiated by a satellite is not required to take a direct path to 
a receiver. If the signal encounters an electromagnet!cally reflective object, it may 
bounce off that object and still find its way to the receiver. This signal has now 
traveled a greater distance than the straight line joining the space vehicle and the 
receiver. Because the receiver assumes the direct path is used, this multipath phe¬ 
nomenon can introduce pseudorange errors. Due to the satellite/receiver geometry, 
multipath is far more likely at low elevation angles. The GPS system has several 
attributes that minimize multipath effects [Ref. 10]: 

• The L band frequency (1227.6 MHz) tends to undergo diffuse rather than spec¬ 
ular reflection. 

• The receiver antennas tend to reject multipath signals. 

• The navigation message is broadcast with circular polarization. Circularly po¬ 
larized signals undergo reversal upon reflection. 

• GPS receivers generally use mask angles (the elevation angle below which the 
satellite is ignored) of at least five degrees. 

All of these factors tend to attenuate the strength of any reflected signal making the 
multipath effect insignificant. Therefore, it is not modeled. 


118 



6. Receiver Noise 

All receivers corrupt the signals they receive. GPS receivers are no excep¬ 
tion. Inaccuracies resulting from quantization error, loop tracking errors and other 
hardware inadequacies corrupt the pseudorange accuracy. According to [Ref. 10], 
representative GPS receiver noise has a standard deviation of 7.5 m. Receiver noise 
is thus modeled as a zero mean, white Gaussian noise with 7.5 m standard deviation. 
As the dilferential station and the aircraft receivers are assumed identical, both air¬ 
craft pseudoranges and differential station pseudoranges are contaminated with this 
noise. The noise in the two receivers is assumed to be independent. 

7. Dilution of Precision 

All of the errors discussed to this point directly effect pseudorange accu¬ 
racy. The various delays and noise sources can cause the receiver’s evaluation of range 
to the space vechicle’s to be inaccurate. However, the pseudoranges themselves are 
irrelevant. The science of navigation is concerned with positioning. Dilution of Pre¬ 
cision is the effect that links pseudorange accuracy to position accuracy. DOP can 
be further classified into several different types: 

• VDOP — Vertical DOP (z) 

• HDOP — Horizontal DOP (x,y) 

• PDOP — Position DOP (x,y,z) 

• TDOP — Time DOP (t) 

• GDOP — Geometric DOP (x,y,z,t) 

The equation which relates DOP and pseudorange error is 

CTp = DOP - (To, (8.17) 
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where ap is the standard deviation of the position error and ctq is the standard devia¬ 
tion of the pseudorange error, often called the User Equivalent Range Error (UERE). 

Dilution of Precision is a function of satellite to receiver geometry. As 
Figure 8.7 shows for a four satellite constellation, GDOP is minimized with the space 




POOR GDOP 
satellites bunched 
together 


GOOD GDOP 
(ideal case) 

one satellite overhead 
3 on horizon, 

120° apart in azimuth 


Figure 8.7: Dilution of Precision from [Ref. 5, p. 4.22] 


vehicles spread out as much as possible. In fact, the volume of the tetrahedron 
formed by the unit vectors from the receiver to each satellite is an empirical measure 
of DOP. PDOP is inversely proportional to the volume of the tetrahedron. If, for 
example, all of the space vehicles a receiver was using for positioning were in the 
same plane, PDOP would approach infinity (the volume of the tetrahedron would be 
zero). Likewise, minimum PDOP is achieved with the geometry shown at the right 
of Figure 8.7. 

The goal for the design of the entire GPS constellation of satellites is a 
PDOP no greater than six everywhere on the earth. With the entire set of 24 space 
vehicles now in orbit, users can expect PDOP values under three [Ref. 20]. 
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The entire concept of GPS navigation has now been thoroughly discussed. 
All of the information put forth in this discussion will be used in developing the DGPS 
computer model in the next chapter. To complete the sensor discussion, a description 
of INS follows. 

F. INERTIAL NAVIGATION 

Inertial navigation has long been the standard for self-contained, long-range 
aircraft navigation. An Inertial Navigation System (INS) senses aircraft thrust accel¬ 
eration, angular rates and spatial orientation resolved in an orthogonal system and 
computes the inertial acceleration. This acceleration can now be integrated — once 
to find velocity, and twice to find position. 

The primary component in any inertial navigation system is the Inertial Measur¬ 
ing Unit (IMU). The IMU is composed of three accelerometers, three rate gyros, and 
two inclinometers. The accelerometers measure thrust acceleration. Thrust accelera¬ 
tion is composed of linear, centripetal, and gravitational effects. Einstein’s Principle 
states that it is impossible for a sensor to distinguish between the effects of gravity 
and acceleration. Thus, the thrust acceleration that it provides is 

X +V, (8.18) 

where is the thrust acceleration, % is the linear acceleration, i; is the angular 
velocity, is the velocity, and is gravity. All quantities are in the body frame. This 
principle necessitates that the computation portion of the INS compute and remove 
local gravity from the measured accelerations. 

Rate gyros sense angular velocities. These angular velocities can be resolved 
in either the body or inertial frame, depending on the type of IMU implementation. 
The two inclinometers sense aircraft inertial orientation, i.e., Euler angles. 
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There are two conceptual methods for implementing inertial navigation: 


• Gimbaled IMU 

• Strapdown IMU 

Brief discussions of each method follow. 

1. Gimbaled IMU 

A gimbaled IMU rotates about its four gimbals during operation (see Fig¬ 
ure 8.8). Aircraft IMU’s must have four gimbals to prevent gimbal lock, while earth- 

Outer 



Figure 8.8: Gimbaled IMU from [Ref. 15, p. 193] 

bound IMU’s require only three gimbals [Ref. 21, p. 192]. A controller maintains 
the IMU in a constant inertial orientation toward true North and lying in the locally 
horizontal plane. By maintaining this orientation, the gimbaled IMU measures iner¬ 
tial quantities directly. This data can be integrated without transformation to yield 
inertial velocity, position and Euler angles. From a navigation standpoint, this con¬ 
figuration seems ideal. However, gimbaled systems are large and heavy, making them 
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impractical for small aircraft. Also, kinematic quantities resolved in the aircraft-fixed 
coordinate system, necessary for stability augmentation and control, are not directly 
available. Instead, they must be computed through a series of Euler rotations. The 
computations required take time and thus introduce delays into an often time critical 
control problem. This fact makes the gimbaled system less than desirable for control. 

2. Strapdown IMU 

Strapdown IMU is conceptually the reverse of the gimbaled system men¬ 
tioned above. Rather than maintaining a constant inertial orientation, a strapdown 
system is “strapped down” to the aircraft, thus maintaining a constant orientation 
in the aircraft-fixed coordinate system. Therefore, the output of the IMU is resolved 
in the local coordinate system. Kinematic quantities are immediately available for 
the control system. The extra computational burden now rests on the navigation 
computer which must transform the accelerations and angular velocities sensed in 
the local coordinate system to the inertial system. Currently, most inertial systems 
are of the strapdown variety. With the advent of high speed, low cost, lightweight 
computing power, the required calculations in transforming from the aircraft-fixed 
to the inertial coordinate system are no obstacle to navigation. This is the system 
which is modeled in this thesis. 

G. INS COMPUTATIONS 

In the strapdown configuration, the IMU measures angular velocities and thrust 
acceleration in the body frame, as well as Euler angles. However, the INS must provide 
position and orientation, both in the inertial frame. In order to compute position and 
orientation in the inertial, tangent plane coordinate system, inertial accelerations and 
Euler rates must be calculated. Before converting the inertial acceleration from the 
body frame to the inertial frame, it is necessary to compute the inertial orientation. 


123 



Computing the Euler rates is a tricky endeavor. The Euler rates are simply related 
to the body angular rates by Poisson’s equation 


■ $ ■ 
0 


■ 1 

0 

sin$ tan0 
cos $ 

cos $ tan 0 
— sin $ 


’ P ' 
? 

, (8.19) 

. ^ 


0 

sin$ sec0 

cos # sec 0 


r 



where p, q, and r are the components of the body’s angular velocity. Obviously, 
this formula requires the exact Euler angles. However, one can only measure these 
angles directly at very low frequency. At frequencies greater than a fraction of a Hertz, 
one must integrate the Euler rates found from Equation 8.19 to find the angles. This 
presents the seeming paradox of needing to know the Euler angles in order to find 
the Euler rates which must be integrated to find the Euler angles. This process 
must be implemented recursively. That is, the results of the integration to find the 
angles must be fed back into Equation 8.19. Furthermore, a complementary Kalman 
filter is necessary to provide an optimal estimate of the Euler angles, trusting the 
inclinometers at the low frequencies and the rate gyros at high frequencies. 

Now having inertial acceleration in expressed in the body frame and the orien¬ 
tation of the body (Euler angles), the transformation from body to inertial can be 
executed. This coordinate transformation is defined by 

“P= ^C''’a + “p, (8.20) 

where 0, and ^ are the roll, pitch and yaw Euler angles, respectively, “P is the 
aircraft’s acceleration in inertial coordinates, is thrust acceleration in the aircraft- 
fixed coordinate system, “p is gravity in inertial coordinates and j C is the transfor¬ 
mation matrix from the body-fixed coordinates to inertial tangent plane coordinates 
as follows 

cos $ cos 0 cos $ sin 0 sin $ - sin cos # cos $ sin 0 cos $ -)- sin $ sin $ 

— sin cos 0 - sin 0 sin $ sin $ -f cos cos $ — sin 0 cos $ sin- cos'S? sin $ 

sin 0 — cos 0 sin # — cos 0 cos $ 
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It is “P which can be integrated to provide velocity and position in the inertial, 
tangent plane frame. 

H. INS ERROR SOURCES 

The Inertial Measuring Unit is subject to a few main error sources. These are: 

• bias 

• cross-axis sensitivity 

• noise floor 

All of these errors apply to both accelerometers and rate gyros. Brief descriptions of 
these problems follow. 

1. Biases 

A bias in an accelerometer or a rate gyro is defined as a constant offset of 
the output of the device from the true value. In other words, an accelerometer might 
constantly read 0.01 m/s^ while the device is not accelerating. This 0.01 m/s^ would 
be referred to as a bias. This error is modeled with a series of small step functions, 
one for each accelerometer and rate gyro. 

2. Cross—Axis Sensitivity 

Cross-axis errors are caused by misalignment of the IMU with the aircraft 
coordinate axes. Ideally, the components of the IMU — the accelerometers and the 
rate gyros — would each be perfectly aligned with the three axes of the aircraft-fixed 
coordinate system. Unfortunately, even the highest fidelity inertial sensors are never 
precisely coincident with the appropriate axes. Misalignment of both types of devices 
causes errors. The cross-axis errors are modeled with the following equation: 

0 ey ez ' 

dca = ex 0 ez a, (8.21) 

ex ey 0 
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where a^a is the cross-axis error, ex, ey, and ez are the cross-axis error terms and 
a is the actual acceleration resolved in the aircraft coordinate system. The e’s from 
the previous equation are determined by the amount of angular offset of each sensor 
from the correct position. 

3. Noise Floor 

All sensor devices corrupt the quantities they measure. Various factors 
including thermal noise can cause a constant output of white noise, regardless of the 
actual acceleration (or angular velocity). This noise floor makes accelerations below it 
not measurable. That is, the output of the sensor still includes the actual quantities, 
but it is “invisible” because the noise floor obscures it. This process is modeled by in¬ 
troducing a threshold to the actual acceleration in the aircraft coordinate system and 
adding white noise. Taking these steps result in the accelerometer always reporting 
noisy, zero-mean acceleration unless the actual value is above the threshold. When 
the actual value exceeds the threshold, that value is added to the noise floor value 
(and the bias) to create the output of the accelerometer. This process also applies to 
the rate gyros. Now both of the sensors — GPS and INS — have been completely 
investigated. 
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IX. KALMAN FILTER DESIGN 


First, recall the general process for filter design: 

1. Obtain navigation equations: 

X = f{x,u) 

y = h{x,u) (9.1) 

2. Linearize 9.1 at (xo,no): 

Sx = A6x + BSu 

6y = CSxADSu (9-2) 

3. Now, we need to use 9.2 to design a filter: 


i) Use 9.2 to create a synthesis model: 


Sxs — AgSXs + Bis^w + B2 sSu 
^ys — CgSxg A 


where 


Sx Q 


Sx 

Sxt 


Sxt are the states resulting from appended integrators. 


Sx 
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Sx 

1 J 

-V-^ 

_ Sxt 


+ Sv 


(9.3) 
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(9.4) 




Figure 9.1: Synthesis Model 


ii) Let W = I and V = I (intensity) where: 

E [6w{t)8w{tf) = W6{t-T) 

E ~ V8{t — T) 

and solve the Ricatti equation for the Kalman gain: 

A.X + XA^ - XCjV-^CsX + BuWBl = 0 

Hs = XCjV-'^ ^ [H Hi] 

iii) Form the filter: 

j 8x ^ As8xs + Hs i8y - 8y) 

\ iy = CSi. (*’• 5 ) 

iv) Now compute the transfer function from 8y to 8y. 

v) If the bandwidth is not sufficient to meet the design criteria, go back to 
step (ii) where a new value of V is chosen. 


4. Now implement the filter on the non-linear plant: 


= f{xs,u)AHs{y-y) 

y (^ s ? ^) 


(9.6) 
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Figure 9.3: Tangent Plane Mechanization 



Figure 9.4: Body Frame and Tangent Plane Relationship 

Now let’s apply this process to Kalman filter design for tangent plane navigation. 
1. Obtain navigation equations: Let 
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Figure 9.5: Pseudorange 


II T3\\1 1 


- ^ IIPIP 

Therefore, using our notation we get 


M + 5(P) = 


where = '"Pof Note 9.7 is in the form 


= /(x, u), where x = 


and u = “A 


Now, we need to include the pseudorange equations 


Pi — ^ Psl "h c^t 


3- cq 







where cAt = UERE (User Equivalent Range Error). 

Note, all computations here must be done in {e}, since satellite positions are 
given in ECEF. 

Let IR be given; Computing IR can be done by selecting the latitude and 
longitude for the origin of {u}. Then: 

where 

0 

^Po. = IR 0 

Ro _ 

Now, 

p, = II IR “Po, + - ^p..|| + cAt (9.9) 

Finally, we need to compute At. Since the only real uncertainty in UERE is 
receiver clock error, we get; 

hi&s 

draft 

Figure 9.6: User Equivalent Range Error Mechanization 



or 


= Ad-\-Wbias 
Ad = Wdrijt 


(9.10) 
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) It can be shown that given y = ||-RP||, where R~^ — RF 


dP^ \\RP\\^^^^ 


Using 9.14 we get 
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Collecting terms, we obtain the linear error equations: 
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(9.14) 
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Note, Equations 9.15 and 9.16 are in the form: 


^ 6x = A6x + B6u j Sw 
, Sy = CSx 


3. Use 9.17 to design a Kalman filter. 


(9.16) 


(9.17) 
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X. GPS/IMU INTEGRATION 


Now that the kinematic equations and coordinate systems have been discussed 
in detail, the next step is to design the navigation system using 'bAMl'Lk'Q/Simulink 
software development tools. The aircraft chosen to evaluate the design, was the 
Airplane D, a medium size, high performance business like jet from Roskam [Ref. 4, 
Appendix C]. Mission requirements for the design were consistent with a medium- 
to-high altitude reconnaissance aircraft cruising in straight and level flight, taking 
high resolution photographs of various military and industrial sites. Flight condition 
2 from [Ref. 4, Table C4] specifies a maximum weight of (13,000 lbs.), high altitude 
(40,000 ft) cruise, at 677 feet per second (M=0.7). At this condition, the aircraft has 
a pitch angle of 2.7 degrees (0.047 radians), a trim lift coefficient of 0.41 and a trim 
drag coefficient of 0.0335. Additional aircraft data includes a wing area of 230 square 
feet, a wing span of 234 feet, a mean geometric chord of 7 feet and a center of gravity 
(c.g.) at 0.32 mean aerodynamic chord (MAC). The stability and control derivatives 
used are presented in Roskam [Ref. 4, p. 620] 

A. EQUATIONS OF MOTION 

The first step in the design of the integrated system is to develop the appropriate 
equations of motion (EOM) for the aircraft. A right-handed body coordinate system 
{6} was chosen referenced to a universal frame {u} with a NED orientation. The 
derivation of the EOM’s starts with the following four equations: 

-V, = - X Vi + — 
at m 

A = 5(A) Vi 
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-p, = ic^Vk 

where 

F = Forces 
N = Moments 

The force and moment terms consist of the thrust, aerodynamic and gravity 
terms. Assuming the moment about the c.g. due to gravity is zero, the force and 
moment equations reduce to: 

F = Ft FaF Fg 
N = Nt + NA 

where 

T = Thrust 
A = Aerodynamic 
g = Gravity 

The thrust terms are computed from the engine thrust settings and aircraft geome¬ 
try and the gravitational force is computed by resolving the gravitational constant 
32.174^ from the universal to the body frame. 

The aerodynamic forces are calculated using a first order approximation of the 
stability and control derivatives contained in Roskam [Ref. 4]. Starting with funda¬ 
mental aerodynamic force equation: 

F = ^-pV^SCp (10.1) 

where 

r = [-T>,-F, -i,/,m,n] (10.2) 
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the first order linear approximation becomes: 

where 

r 

a; = u V w p q r 

x' — — u V w ^ ^ br ] 

Vt '■ ^ ^ j 

= ii' a' p' q' r' 

S = diag [-S S -S Sb Sc Sb' 

A = 6e Sa Sr ST ] 

The initial development of the EOM involves only the and ■^’’ujb equations. 
In matrix form this becomes the basic EOM equation: 


■ 1 [ x^Vb 1 r ic'^g ■ 

_ ^ J [ ( ^Ub ( ’’ub X (h X (4 ^u;^)))) J + [ 0 


where 4 is the aircraft’s inertia tensor and the aerodynamic forces and moments 
are computed in the wind reference frame {w} and must be converted to the body 
frame {b}. The first order approximation of the aerodynamic forces and moments was 
substituted into the basic EOM equation and the A equation was added yielding the 
complete EOM equation. After the trim values were computed, they were included 
as initial conditions in the EOM’s integrator. The diagram was then linearized, 
using the MATLAB linmod.m command. EOM verification was done by comparing 
the eigenvalues of the linearized model to Roskam’s data. The results are shown in 
Table 10.1. 
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TABLE 10.1: EIGENVALUES 



Roskam 

EOM 

Long period 

-0.00593 ± 0.0904i 

-0.0103 ± 0.09122 

Short period 

-0.9943 ± 2.641i 

-0.7941 ± 2.7142 

Spiral 

-0.0012 

+0.0007 

Dutch roll 

-0.05848 ± 1.6842 

-0.0567 ± 1.6862 

Roll mode 

-0.5023 

-0.5036 


The linearized values compare quite nicely to Roskam’s values except for one 
unstable eigenvalue in the lateral dynamics. This pole corresponds to the spiral mode 
and has a large time constant. It does not affect stability in the longitudinal dynamics. 
Finally, the three position states were added to the initial EOM model to make the 
final EOM model, shown in Figure 10.1, which is used as an input to the IMU System. 



Figure 10.1: Equations of Motion Simulink Block Diagram 
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B. INERTIAL MEASUREMENT UNIT (IMU) 

In order for the IMU to calculate position and velocity in the universal frame u, 
the aircraft’s angular rates about the a;, y, and z axes (p, g, and r respectively) and 
thrust accelerations in a^-, Uy, and a^, must be measured. A real IMU uses angular 
gyros and accelerometers to provide this data, where the design IMU model, Fig¬ 
ure 10.2, uses state vector outputs from the EOM model to perform the acceleration 


IMU model (Level 1) 



Figure 10.2: Inertial Measurement Unit (Level 1) Simulink Block Diagram 

calculations. The IMU inputs consist of the linear velocity components (u, v, w) or 
Vb , angular rates {p,q,r) or ujb , Euler angles (^,^,■0) or A , and the position of the 
body in the universal frame, Additionally, the linear accelerations, Vb, are also 
provided to complete the calculations. ’Given the small size of the aircraft, it was 
assumed that the IMU was located at the aircraft’s center of gravity and all calcu¬ 
lations are based on this assumption. Since real IMU sensors possess some inherent 
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noise and bias characteristics, white gaussian noise was included to simulate the noisy 
sensors and a nominal bias to account for the design limitations of the sensors. 

To calculate the accelerations in the {u} frame, Figure 10.3, the gravity force 


IMU model (Level 2) 



Figure 10.3: Inertial Measurement Unit (Level 2) Simulink Block Diagram 

was calculated and a transformation matrix from {«} to the body frame {6} was 
applied: 

(10.3) 

The accelerations in the body frame where calculated and the acceleration due 
to gravity was removed: 

'a = ^ X Vfc - (10.4) 

The MATLAB routine to perform these calculations was accel.m and is provided 
in Appendix B. After the accelerations in the body frame were calculated, the noise 
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and bias errors were incorporated to simulate actual acceleration measurements from 
the IMU. Since they were calculated in the body frame, the noisy accelerations, *’a„, 
were transformed to the universal frame using a transformation matrix, ^(7. The 
MATLAB function used to perform this calculation was abn2un.m . The resulting 
outputs from the IMU were noisy accelerations in the universal frame, 

C. GLOBAL POSITIONING SYSTEM (GPS) 

In order to provide accurate position information that could be used to up¬ 
date or verify the position estimates calculated from the IMU accelerations, a GPS 
mechanization was designed. The GPS mechanization. Figure 10.4, was designed to 


GPS model 



Bias1 


Figure 10.4: GPS Pseudo—range Simulink Block Diagram 

provide a pseudo-range measurement based on the aircraft’s estimated position and 
position of four satellites. Inputs to the GPS came from the EOM linear veloc¬ 
ity terms, Vh , which were transformed from the body frame to the universal frame 
and integrated to provide a position estimate, . Satellite positions were input 
as a constant value using a satellite initial position MATLAB function, initsat.m . A 
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MATLAB function, exact.m , calculated the exact ranges from each satellite. User 
equivalent range error (UERE) was determined by introducing white gaussian noise 
to simulate the GPS receivers inherent noise bias and clock drift. Combining the 
exact range with the UERE, a pseudo range value was calculated. Along with the 
noisy accelerations calculated in the IMU, the pseudo—range values were input to the 
Kalman filter. 

D. KALMAN FILTER 

The Kalman filter was designed for the purpose of providing optimal inertial 
velocities and positions given noisy accelerations from the IMU and pseudo-ranges 
from a GPS receiver. To accomplish this, it was desired to have a break frequency of 
1^ as the cutoff below which only GPS information would be valid. This constituted 
steady-state, straight and level flight. For a maneuvering aircraft, frequencies greater 
sampling rate of the GPS would not be capable of providing adequately 
up-dated position information. At this point, the accelerometer data would be con¬ 
sidered valid. A design criterion of 1 ^ as a -3dB cutoff for the frequency response 
was chosen to meet these requirements. 

1. Navigation Model 

The first step in the filter design was to develop a navigation model which 
provided velocity and position as outputs. Using the position outputs, pseudo-ranges 
could be calculated. The navigation model was divided into two components. The 
main component. Figure 10.5, received the noisy accelerations from the IMU which 
were then integrated twice to produce velocity and position in the universal frame. 
The second component. Figure 10.6, calculated estimated pseudo—ranges to the four 
satellites given their position, the aircraft’s estimated inertial position based on the 
accelerations, and a clock drift error component. The output of the navigation model 
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Exact range 


Figure 10.5: Navigation Model Simulink Block Diagram 


Exact Range 



Noise2 


Figure 10.6: Kalman Filter, Pseudo-range Simulink Block Diagram 

was four estimated pseudo-ranges. These estimated ranges were compared to actual 
pseudo-ranges from the GPS receiver. 
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2. Linearized Model 

The next step was to linearize the navigation model at a nominal aircraft 
position. This was accomplished by using the MATLAB function, linmod.m . on the 
navigation model. The linearized state space matrices, [A, B, C, D], were then used in 
the development of the synthesis model. In state space form, the linearized equations 
become: 

6x — A6x + B6u 
Sy = C6x + DSu 

Here D = 0. 

3. Synthesis Model 

The next step in the design of the Kalman filter, was to create a sythesis 
model which accounted for measurement and sensor noise inputs. In this case only 
sensor noise bias was considered. By considering only noise bias in each accelerometer 
axis, only one integrator per axis is required. These appended integrators result in 
three poles at the origin. As the gains are increased, the poles will diverge in three 
directions. One on the negative real axis and the other two will depart at 45 degree 
angles out the left—half plane. To insure system stability, three zeros are included 
so that the poles converge to a value along the negative real axis. In the synthesis 
model, zeroes were chosen at -1, which forces the poles to converge on the negative 
real axis. In addition to the noisy inputs, the accelerations in each axis are included 
as inputs. The resulting synthesis model is shown in Figure 10.7. Next, the synthesis 
model is linearized to determine the synthesis state space matrices, [A^, D*]. 

The resulting state space form is: 

Sxs = AsSxs + Bi^Sw + B2^Su 

6y = CsSxASv (10.5) 
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Mux 


Figure 10.7: Synthesis Model for Kalman Filter, Simulink Block Diagram 

In the equation 10.5, 6v is zero since only input noise is considered. 

4. Noise Intensity and the Non-Linear Plant 

In order to solve the Ricatti equation 10.6 for the optimal gain 

A.X + XA^ - XCjV-^CsX + = 0 (10.6) 

the noise intensities V and W must be adjusted to provide the optimum frequency 
response. The initial values were chosen to be identity matrices, I. Once the val¬ 
ues were chosen, the MATLAB function, Iqe.m , was used to determine the optimal 
Kalman gain, Hs- The Kalman gain Hg was partitioned based on the arrangement of 
the states in the Bg matrix. The partitioned Kalman gains were implemented on the 
non-linear plant shown in Figure 10.8 
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Hbias 


Figure 10.8: Non-Linear Kalman Filter 
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Next, a bode plot of the frequency response was generated and the band¬ 
width’s were observed. The process was repeated using a function called, kal.m . This 
allowed the weighting factors to be adjusted until the optimum frequency response 
was obtained. It was necessary to decrease the weighting factor on the outputs V and 
increase the weighting on the inputs W. The optimal bandwidth of 1 ^ was acheived 
at V and W values of 0.057 and 67, respectively. The frequency responses from the 
pseudo range to the pseudo range estimates are shown in Figures 10.9 and 10.10. It 




Figure 10.9: Frequency Response, Pseudo-range to Pseudo-range Estimate 
1 & 2 

can be seen that the -3dB cutoff occurs at exactly 1 ^ for each input. The frequency 
responses of the filter to accelerations in the x, y and z directions are shown in Fig¬ 
ure 10.11. The 20dB per decade slope of the curve below the frequency of 0.1 
indicates the accelerometer inputs are washed out at low frequencies. This is within 
the design criteria. To verify the response of the filter to a disturbance in accelera¬ 
tion, a step input was performed in each channel and the pseudo-range responses are 
shown in Figure 10.12. 
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Figure 10.10: Frequency Response, Pseudo-range to Pseudo-range Esti 
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Figure 10.11: Frequency Response, Accelerations to Pseudo-range Esti 
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XI. CONCLUSION 


The purpose of this thesis has been to provide the engineer with a solid physical 
and mathematical understanding of the relationship between the GPS/IMU sensors 
and the air vehicle and how to integrate the two systems properly. 

The extensive use of 3-D graphics for depicting the coordinate relationships and 
the corresponding transformations, gives meaning to the physical and mathematical 
relationships between the two systems. By introducing the fundamental concepts 
behind inertial sensors and global positioning systems, the data each provides can 
be understood and integrated properly. Next, the errors that are common to both 
systems are discussed and error analysis is applied to reduce the effects on the output 
of the integrated system. Introducing the design method for Kalman filtering allows 
the engineer to develop a system which accounts for the inherent errors and guarantees 
positional accuracy no matter which sensor is providing the information. Developing 
the design through the use of MATLAB/<S?77iu/ira^ software, allows the engineering 
team the opportunity to apply all of the concepts introduced in this thesis and gives 
them the flexibility to expand on the basic concepts. 

This thesis has provided the uniform framework required in the design of an in¬ 
tegrated GPS / IMU system using Kalman Filtering. As GPS and IMU components 
become smaller and more lightweight, their use in Unmanned Autonomous Vehicle’s 
(UAV’s) will increase dramatically. The need for precise navigational data will con¬ 
tinue to rise as the requirements for UAV’s increase to meet their predicted threat. 
Providing the engineer with a complete document which approaches the design pro¬ 
cess in a step-by-step manner, is essential if design team is to compete effectively. 
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APPENDIX A: SHOWCASE, SNAPSHOT, 
AND XV UNIX SOFTWARE 

The graphical models used in the development of this thesis were produced 
using the Silicon Graphics Indigo computers available in the Aeronautical Engineering 
Computer Laboratory and the Avionics Design Laboratory, both located in Halligan 
Hall. The software utilized to produce the 3-D models was IRIS Showcase (version 
3.3). 

A. IRIS SHOWCASE 3.3 

To access the Showcase graphical program, type “showcase” at the prompt in 
the winterm window. Three windows will appear when “Showcase” comes on-line. 
The first and largest window will be the drawing tablet, figure A.l. This window 



Figure A.l: Showcase Drawing Tablet 


is used for producing the actual picture. The next window is called the “Showcase 
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Master Gizmo”. This is the main tool for selecting 1-D drawing devices, figure A.2. 
The third window is a “status gizmo”, figure A.3, which displays the status of each 



Figure A.2: Showcase Master Gizmo 
command selected or action performed in “Showcase”. 



Figure A.3: Showcase Status Gizmo 

1. Creating 1—D and 3—D Objects 

To draw 1-D figures, select the appropriate tool from the “Master Gizmo” 
using the mouse “point and click” method. The method is similar to all graphics 
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programs used on Macintosh and IBM computers. Type styles can be selected from 
the same tool-box and fonts can be chosen at the lower portion of the “Master Gizmo” 
window. These operations are self-explanatory. 

To draw 3-D objects, the strength of this program, go to the “Gizmo” pull 
down menu at the top of the drawing tablet. Select the “3-D” option from the menu 
and “3D Gizmo”, figure A.4, will appear. Using the mouse arrow, select the “Create” 



Figure A.4: Showcase 3D Gizmo 

tool in the upper right corner of the “3D Gizmo” window. Next, place the mouse 
arrow in the drawing area of the tablet. A red box will appear and move about 
the window as the pointer is moved. Place the window in a selected position and 
click the left mouse button to drop the box. A 3D room will appear on the page. 
This is the drawing surface for the 3D object, figure A.5. Next, return to the “3D 
Gizmo” and select any of the Simple models using the “point and click” method. 
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Figure A.5: Showcase 3D Drawing Window 


Tubular and beveled surfaces can be chosen also. The Faceted and Smooth sliding 
scale tool adjusts the number of surfaces the object will have. The smoother the 
surface, the more memory the object will require. Moving the mouse arrow back to 
the 3D drawing window, place the pointer in the appropriate spot and hold the left 
mouse button down while dragging the object outward, figure A.6. In the case of a 
sphere, the size of the radius depends on how far the pointer has been dragged from 
the initial position. Selecting the Material and Texture of the object produced is self 
explanatory and well worth the time spent experimenting. Once the 3-D object has 
been produced, the orientation of the object within the 3-D drawing window can be 
manipulated using either the “hand” tool on the “3D Gizmo” window or by placing 
the mouse arrow on any surface “wall” inside the 3-D drawing window and holding 
left mouse button down while moving the mouse. The room will rotate to the desired 





















Figure A.6: Showcase 3D Sphere 

orientation. 

Once the 3-D model has been produced, place the mouse arrow outside of 
the 3-D drawing window and click the left mouse button. The drawing window will 
disappear and 1-D objects can once again be produced on or around the 3-D object, 
figure A.7. 

2. Saving the Drawing 

The object can be saved in a variety of formats. The default style is a 
“Showcase” document. All files should be saved in this format so that they can 
edited at a later date. Encapsulated Postscript (EPS) and Postscript (PS) styles 
are the style of choice, for they can be imported into a variety of word processing 
programs. However, some programs do not allow the operator to choose the size of 
the object when it is imported into the document. Since this thesis was produced 
using DTgXtypesetting software, both styles will be discussed. 
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Figure A.7: Showcase 3D Sphere in ID Environment 


3. Encapsulated Postscript 

Once the object has been created, return to the pull down menus on the 
drawing tablet. Select the “Edit” pull down menu and choose the “Select All” func¬ 
tion. This highlights all objects in the drawing window. Next, choose the “File” pull 
down menu and select the “Save as” function. Moving the mouse right an additional 
pull down menu will allow the operator to select the type of file format which the 
object will be saved as. Choose the “...as EPS” function. A “Showcase File Browser” 
window will appear and the file name can either be chosen or typed in at the bot¬ 
tom of the window, figure A.8. The only problem with importing EPS files into a 
lATgXdocument is that the size of the object cannot be adjusted using “height and 
width” commands where they can be if a PS file is used. 

4. Postscript 

The same procedure is used for saving the object as a PS file. However, 
when saving a Postscript file, the default device the file will be saved to is the printer. 
An additional menu will appear which will allow the operator to choose whether to 





Figure A.8: Showcase File Browser 
print the object to a file or the printer, figure A.9. 





1 


Figure A.9: Showcase PS Save Window 


5. Additional 3D Goodies 

A slide show presentation function is available by selecting the “Page 
Gizmo” from the “Page” pull down menu on the main drawing tablet, figure A. 10. 
Experimentation is the best way to learn the system! 

B. SNAPSHOT 

Including graphics from the display requires that they be converted into a 
Postscript file. To do this, the SGI computers have the capability to take a “snap- 
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Figure A. 10: Showcase Page Gizmo 


shot” of the graphic and save it as a “*.rgb” file. This file extension is the default 
to the SGI system. This is accomplished by typing “snapshot” at the winterm win¬ 
dow. A small window will appear in the upper left portion of the display, figure A.ll. 
Place the mouse arrow on the “Snapshot” window and depress the left mouse button. 



Figure A.ll: SGI Snapshot Window 


Drag the mouse over the object that is to be copied. A red box will position around 
the object. The box size can only be controlled when the mouse arrow is on the 
“Snapshot” window. If the edge of the box does not move, the mouse arrow must be 
re-positioned on the window. Once the object has been boxed, place the mouse ar¬ 
row on the “Snapshot” window and depress the right mouse button. A small window 
will appear which will allow the object to be saved as a “*.rgb” file. Name the file 
accordingly and then “save” or “save and exit” the program. 


































C. XV 


Once the object has been saved as an “’*‘.rgb” file, it can be converted to 
Postscript format by using the “XV” program on the SGI computers. Type “xv 
<filename.rgb>” at the in figure A.12. 



Figure A.12: XV Window 


Place the mouse arrow on the picture and click the right mouse button. An 
“xv controls” window will appear, allowing the operator to save the file, figure A. 13. 
Select the “Save” button on the “xv controls” window. A small fish will swim in circles 



Figure A.13: XV Controls Window 


until the “xv save” window appears, figure A. 14. Select the “Postscript” button on 
the lower left side of the window, as well as the “B/W Dithered” button, if the file 
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Figure A. 14: XV Save Window 

is to be saved in black and white. Choose the file name to save and select the “Ok” 
button in the upper right corner. The file can now be accessed as a Postscript file 
and imported into any word processing program that accepts that format. 



D. SHOWCASE PRESENTATIONS 

Through the use of “Showcase” two animations have been developed to provide 
a visual depiction of the coordinate systems relationships and the coordinate transfor¬ 
mation from navigation to body frames. These are provided on the following pages. 
They can be accessed through the “Showcase” browser window. 
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Figure A.18: Coordinate Transformation, Slide 4 
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Rotation about Zi 


cX sX 0 
-sXcX 0 
0 0 1 



Figure A.20: Coordinate Transformation, Slide 6 
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Figure A.23: Coordinate Treunsformation, Slide 9 
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Figure A.28: Navigation to Body Rotation, Slide 5 
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Is this a positive 
or negative rotation? 
Does it matter? 



Yb 

Yn 


Zn Zb 


Figure A.34: Navigation to Body Rotation, Slide 11 
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Now rotate about 
Yn. This aligns 
the X axes. 

Is this a positive 
rotation? Right Xn 

hand rule! Xb 



Zn Zb 


Figure A.36: Navigation to Body Rotation, Slide 13 
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Now rotate about 
the Xn axis* This 
will align the y 
and z axes* 



Zn 


Figure A.37: Navigation to Body Rotation, Slide 14 
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APPENDIX B: MATLAB FUNCTIONS 


* :ic 5jc :f: :ic afc *** :^c sjc 5f: s^c 5|C :f: :f: J(c Sf: ***** 5f: ***** :^c 5*C :i}c 5ic ****♦:*: jf: :jc ****** :|c *** sjc i|c jf: sjc 5jc 3j: ;f: :ic :f: 

MATLAB functions used in the IMU / GPS Integration 

**********************************************************5f;:JC5j:3j::ie:f;3(C5jC3ic*** 

function y=abn2un(vec) 

% Transform noisy accelerations from body fixed to universal 

Ax=vec(l); 

Ay=vec(2); 

Az=vec(3); 

euler=vec(4:6); 

Tb2u=rb2u(euler); 

y=Tb2u*vec(1:3); 

function Ab=accel(vec) 

% This function calculates the acceleration terms in the body 
% coordinate frame 

% Establish the order of the states 
vbo=vec(l:3); 
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wb=vec(4:6); 

LcLmbda=vec(7:9) ; 

PboU=vec(10:12); 
dvbo=vec(13:15) ; 

y, Calculate the cross product of wb and vbo 
xprod=crpr(wb,vbo); 

% Transform Lambda into the body fixed coordinate frame 
BLambda=ru2b(Lambda); 

% Multiply BLambda by the gravity term gu 
gb=BLambda*gravitya(PboU); 

‘/o Calculate dvbo + (wb X vbo) - Ru2b*gu 
Ab=dvbo+xprod-gb; 

5|C 5)C :if: 3|c 5|C 5fc :)Jc 5|C 5jc 5f: 3|C :|c sjc 5ic rijc sjc +5k * 5|« * ******* SK **** * + ***** **** * 

function y = crpr(omega,x) 

y, This subroutine computes the crossproduct of omega and x 
y. y = omega X x 
p = omega(l); 
q = omega(2); 
r = omega(3); 

t = [0 -r q; r 0 -pj-q p 0]; 
y = t * x; 
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3jc +jf: 5|c ** 5|C Jjc * ** Sf: ********* sf: * ** :1c ***** 3fc :f: 5f: 3}.: :tc 5|c :f: * if: 5(C ** :f: :^c if: ******* * * 


function EDMstate=eoml(vec) 

y. This function will calculate the accelerations due to forces, 
'/o moments, and control inputs. 

% (1) Input aircraft data and initialize velocity terms: 

Vi=677; % Initial velocity (fps) 

W=13000; I Weight 

S=230; y, Wing planform area 

b=34; y Wing spcin 

cbar=7; % Mean aerodynamic chord 

alpha_i=.047; Ji Initial angle of attack (rad) or 2.7 degrees 

beta_i=0; % Initial sideslip angle 

rho=.000588; % Density at 40,000 ft 

g=32.174; y. Gravitational constant 

m=W/g; y Mass of the aircraft 

To=1036.75; '/ Initial thrust 

No=zeros(3,1); 

Ib=[28000 0 -1300;0 18800 0;-1300 0 47000]; y Inertia tensor 


Rw2b_i=[cos(alpha_i)*cos(beta_i) ,-cos(alpha_i)=t'sin(beta_i) ,sin(alpha_i) ; 
-sin(beta_i),cos(beta_i),0; 
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siii(alpha_i)*cos(beta_i) -sin(alpha_i)*sin(beta_i) cos(alpha_i)] ; 


[vbo]=Rw2b_i*[Vi 00]'; 
uo=vbo(l);vo=vbo(2);wo=vbo(3); 


% (2) Input the derivative coefficients due to the states 


dCx=[.104 0 .3 0 0 0; 

0 -.73 0 0 0 .4; 

.40 0 5.84 0 4.7 0; 

0 -.110 0 -.45 0 .16; 
.05 0 -.64 0 -15.5 0; 

0 .127 0'-.008 0 -.20]; 

dCxdot=[0 00000; 
000000 ; 

0 0 2.2 0 0 0 ; 

000000 ; 

0 0 -6.7 0 0 0; 

0 0 0 0 0 0 ]; 

dCdelt=[0 0 0; 

00 .140; 

.46 0 0; 

0 .178 .019; 

-1.24 0 0; 
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0 -.02 -.074]; 


Cfl=[.0335 0 .41 0 0 0] '; 

% (3) Define state variables 
vb=vec(l:3 ); 

u=vb(l); v=vb(2); w=vb(3); 
wb=vec(4;6); p=wb(l); q=wb(2); r=wb(3); 

lambda=vec(7:9); phi=lambda(l); theta=lambda(2); psi=lambda(3); 
cont=vec(l0:12); elev=cont(1); ail=cont(2); rud=cont(3); 
dt=vec(13); 

% (4) Determine total velocity and dynamic pressure 

V=sqrt(u''2 + v''2 + w''2) ; 
qbar=. 5*rh,o*V"2; 

y. (5) Determine angle of attack (alpha) and sideslip (beta) 

alpha=asin(w/V); 
beta=asin(v/V); 

y. (6) Develop diagonal Sbar matrix 

Sbar=diag([-S,S,-S,S*b,S*cbar,S*b]); 
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% (7) Develop Ml and M2 matrices 


Ml=diag([l/V,l/V,l/V,b/(2*V),cbar/(2*V),b/(2*V)]); 

M2=diag(C0, .5*b/V~2, .5*cbar/V''2,0,0,0] ) ; 

% (8) Calculate mass inertia matrix 

MI=[eye(3)/m zeros(3,3);zeros(3,3) Ib\eye(3)]; 

*/, (9) Develop wind to body transformation matrix 

Rw2b=[cos(alpha)*cos(beta) -cos(alpha)*sin(beta) -sin(alpha); 
sin(beta) cos(beta) 0; 

sin(alpha)*cos(beta) -sin(alpha)*sin(beta) cos(alpha)]; 

Tw2b=[Rw2b zeros(3,3); zeros(3,3) Rw2b]; 

% (10) Calculate linear eind angular velocity terms 

SW=-crpr(wb,vb); 

ISIW=(Ib)\crpr(Ib*wb,wb); 

SWM=[SW ; ISIW]; 

Y=MI*Tw2b.*qbar*Sbax*dCx*Ml*[vb-vbo;wb]; 

(11) Calculate universal to body rotation matrix (3-2-1) 

Rpsi=[cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 001]; 
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Rtlieta= [cos (theta) ,0,-sin (theta); 0,1,0; sin (theta) ,0, cos (theta)] ; 
Rphi=[l 0 0; 0 cos(phi) sin(phi); 0 -sin(phi) cos(phi)]; 

Ru2b=Rphi*Rtheta*Rpsi; 

Rb2u=Ru2b'; 

% (12) Calculate gravity and control terms 

Fgravu=[0 0 g]'; 

Fgravb=[Ru2b*Fgravu; zeros(3,l)]; 

% Calculate thrust force terms. Assume thrust acts through the eg 
% and there are no moments due to thrust 

T=[To/m;0;0]; 

Fthrust=[T;(Ib)\No]*dt; 

% (13) Calculate aero forces and moments 

FaeroM=MI*Tw2b.*qbar*Sbar; 

FaeroC=[Cfl+dCxdot*M2*[(crpr(wb,vb-vbo));zeros(3,1)]+dCdelt*cont]; 
F aero=FaeroM*FaeroC; 

7o (14) Calculate chi 
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chi=[eye(6) - MI*Tw2b.*qbar*Sbar*dCxdot*M2]; 


% (15) Calculate accelerations 

E0M1_6 = chi\(SWM + Y + (Fgravb + Fthrust + Faero)); 

% (16) Calculate Lambdadot 

QLambda=[l sin(phi)*tan(theta) cos(phi)*tan(theta); 

0 cos(phi) -sin(phi); 

0 sin(phi)*sec(theta) cos(phi)*sec(theta)]; 

E0M7_9=QLambda*wb; % Lambdadot 


% (19) Calculate state output 

E0Mstate=[E0M1_6;EDM7_9]; 

5»:5|C5jC:<C5t£ ******************************************************** ********* 

function xout = exact(vec) 

y, This function computes the pseudo ranges by adding the clock 
% bias range to the actual range. Input vector consists of the 
y, actual aircraft position (PboU) , the position of each satellite 
y. (Psi), euler angles, user equivalent range error (re) 
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Ro=20.997e6; 

Puoe=[Ro 00]'; % Position of the tangent plane 
y„ (Greenwich meridian) 

y Assume tangent plane origin remains at 
y the Greenwich Meridian. 

Pbou=vec(l:3); */. True aircraft positions from EOM 

Posl=vec(4:6); y Satellite positions in earth fixed frame 
Pos2=vec(7:9); 

Pos3=vec(l0:12); 

Pos4=vec(13:15); 


yRu2e-C0 0 1; y Transform tcingent plane to earth fixed 

yo 10; 

yi 0 0] ; 

Pboe=Pbou + Puoe; y Transform aircraft position from body to 
y earth fixed 


rhol=norm(Pboe+Puoe-Posl); 
rho2=norm(Pboe+Puoe-Pos2); 
rho3=norm(Pboe+Puoe-Pos3); 
rho4=norm(Pboe+Puoe-Pos4) ; 
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xout=[rhol rh,o2 rho3 rho4] '; 


* * :ic * * * * 5fc * * :<c * sjc * * :Jc 5^ 5|c 3(c * j^c :f: * :)c if: + 3fc 3|c * :f; 5jc 5|c * * * if: * 5jc 5jc * * * * * * * * * * 5|c 5jc :f: 5tc jfc * 5l< 5^:* 5j< 3|c 

function g = gravitya(r) 

•/. 

% This function computes the gravity vector in the tangent plane 
y, r is the vector of the vehicle's position in tangent plane 

y. 

RO = 20.997e6; 
mu = 32.2*R0“2; 

R = (r(l)~2 + r(2)''2 + (r(3) + R0)''2)‘.5; 
gx = -(mu/R''3)*r(l); 
gy = -(mu/R“3)*r(2); 
gz = -(r(3) + Rd)*mu/R~3; 
g = [gx; gy; gz]; 

**!('****!t!**********=(:**s(:****!|!S(:**5|ts(!*!(!******+!)c*****!(:****ste*!(:***:t:*:(c****5t:*!t!!|cs(! 

function satpos=initsat 

% Initialize satellite and GPS receiver positions 


Rs=285.885e6; cl=cos(2*pi/3); sl=sin(2*pi/3); s2=-sl; 
satposl=[0 0 Rs]^; satpos2=[Rs 0 0]'; 

satpos3=[Rs*cl Rs*sl 0]^; satpos4=[Rs*cl Rs*s2 0] '; 
satpos=[satposl' satpos2' satpos3’ satpos4']; 
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function out=pos(vec) 

'/« This function converts velocity in the body frame to velocity in 
y. the inertial frame 

vb=vec(l:3); u=vb(l); v=vb(2); w=vb(3); 
wb=vec(4:6); p=wb(l); q=wb(2); r=wb(3); 

lambda=vec(7:9); phi=lambda(l); theta=lambda(2); psi=lambda(3); 
Rpsi=[cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 00 1]; 

Rtheta=[cos(theta) 0 -sin(theta); 010; sin(theta) 0 cos(theta)]; 
Rphi=[l 0 0; 0 cos(phi) sin(phi); 0 -sin(phi) cos(phi)]; 

Ru2b=Rphi*Rtheta*Rpsi; 

Rb2u=Ru2b'; 

out=Rb2u*vb; 

***5(c3lc*5|c:|c**:^:4c***********5}c********************3)c******5f;*3(c5lc**5)c*5*c*3jc5j;5f:**5ic5lc** 

function xout = pseudo(vec) 

‘/o This function computes the pseudo ranges by adding the clock 
% bias range to the actual range. Input vector consists of the 
*/» actual aircraft position (PboU), the position of each satellite 
% (Psi) , euler cingles user equivalent reinge error (re) 


201 



Ro=6.38e6; 

Puoe=[Ro 0 0]’; % Position of the tangent plane, 
y. Assume tangent plane origin remains at 
% the Greenwich Meridian. 

Pbou=vec(l:3); % True aircraft positions from EOM 

Posl=vec(4:6); “h Satellite positions in earth fixed frame 
Pos2=vec(7:9); 

Pos3=vec(l0:12); 

Pos4=vec(13:15); 

re=vec(16); */, User equivalent range error 

Ru2e=[0 0 1; % Transform tangent plane to earth fixed 

0 10 ; 

10 0 ]; 

Pboe=Ru2e*Pbou + Puoe; % Transform aircraft position from body to 
y, earth fixed 


rhol=norm(Pboe+Puoe-Posl)+ re; 
rho2=norm(Pboe+Puoe-Pos2)+ re; 
rho3=norm(Pboe+Puoe-Pos3)+ re; 
rho4=norm(Pboe+Puoe-Pos4)+ re; 
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xout=[rhol rho2 rho3 rho4]'; 


5|c * 5|C sf: *5fc ♦ ****************************************** Jjc* 3fc 4::t:* ************ * 

function y=rb2u(x) 

% Calculate inertial to body rotation matrix (3-2-1) 


phi=x(l); 
theta=x(2 ); 
psi=x(3); 

Rpsi=Ccos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 0 0 1]; 

Rtheta=[cos(theta) 0 -sin(theta); 010; sin(theta) 0 cos(theta)]; 
Rphi=[l 0 0; 0 cos(phi) sin(phi); 0 -sin(phi) cos(phi)]; 

y=[Rphi*Rthet a*Rp si]^; 

**************************************:(c:)c****************************** 

function y=ru2b(x) 

'/o Calculate body to inertial rotation matrix 

phi = x(l); theta = x(2); psi = x(3); 

Rpsi=[cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 0 0 1]; 
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Rtheta=[cos(theta) 0 -sin(theta); 0 10; sin(theta) 0 cos(theta)]; 
Rphi=[l 0 0; 0 cos(phi) sin(phi); 0 -sin(phi) cos(phi)]; 

y=Rphi*Rthet a*Rpsi; 

4:************5fc*****5|c*******5}c*5|c***********4c***5j«:***:+:*:t;ijc>^:?Jc**:t:** **.******** 

% Function kal.m 

y. This function performs the linearization of the Kalman filter. 

% First I will linearize the navigation model "navmod.m". The inputs 
y are Ax, Ay, and Az from the IMU emd drift error for the exact range 
y calculations. Outputs are the four pseudo ranges. 

[a,b,c,d]=linmod(^navmod') 

pause 

y Next, I will include these matrices in the synthesis model, 
y "syn.m". Inputs 1 through 4 will be noisy accelerations where 
y I will append transmissions zeros at -1 cind include an integrator, 
y Input 4 will be noise from the clock model. Inputs 5 through 7 
y will be the accelerations from the IMU. 

[as,bs,cs,ds]=Iinmod('synO 

pause 
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‘/o Next I will determine the Kalman gains from the noise inputs, 1-4. 
% I chose Q=1 and R=1 initially. 

Q=eye(4);R=eye(4); 

l=lqe(as,bs(;,1:4),cs,Q,R) 

% I will check the bode response of the pseudo range inputs to the 
y. pseudo range estimates. I will use a function called "bplots.m". 

bplots 

% I will adjust the weighting values to obtain a bandwidth of 
y 1 rad/sec. 
y Decrease R to 5. 

l=lqe(as,bs(:,1:4),cs,Q,5*R) 
bplots 


l=lqe(as,bs(:,1:4),cs,.5*Q,5*R) 
bplots 


l=lqe(as,bs(:,1:4),cs,.05*Q,50*R) 
bplots 
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l=lqe(as,bs(:,1:4),cs,.05*Q,6*R) 
bplots 

% I will use the values of Q=.05 and R=6. These provide bandwidths 
*/o of 1 rad/sec. 

y. Next, I will look at the response of the filter from the 
y. accelerations (input) to the pseudo rainges (output) . I will use 
y a function called "accplot.m". 

accplot 

** * :>jc 5|c 5f: ** **** s)c * sj: 5ft 5(£5)c :f: 5»£ **** j^c 5(c 5|c ** 5(c * :f: ************************ * 

y, Function bplots.m 

y. This function plots each input bode response 

w=logspace(-4,2,100) ; 
x=-3*ones(size(w)); 

[mag,phase]=bode(as-l*cs,l,cs,ds(:,1:4),l,w); 
magdb=20*log(mag); 
subplot(211) 

semilogx(w,magdb,w,x,'.0 
grid 

title('Bode Response for Pseudo ranges 1 & 2') 
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axis(Cle-4 le2 -10 10]) 
pause 
% print 

[mag,phase]=bode(as-l*cs,l,cs,ds(:,1:4),2,w); 
magdb=20*log(mag ); 
subplot(212) 

semilogx(w,magdb,w,x,'.0 
grid 

axis([le-4 le2 -10 10]) 
pause 
% print 

[mag,phase]=bode(as-l*cs,1,cs,ds(:,1:4),3,w); 

magdb=20=i'log(mag); 

subplot(211) 

semilogx(w,magdb,w,x, ’. 

grid 

title('Bode Response for Pseudo ranges 3 & 4') 
axis([le-4 le2 -10 10]) 
pause 
y, print 

[mag,phase]=bode(as-l*cs,l,cs,ds(:,1:4),4,w); 
magdb=20*log(mag ); 
subplot(212) 
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semilogxCwjinagdbjW^x, ^ . 0 
grid 

axis([le-4 le2 -10 10]) 
pause 
% print 

3(c :4c :)f: :f; :4: :4c 3)e 9^ 3}c 3|c ^ 9)c sic :f; 9f; :4c )(c )f: 4:9f: 9|c 9)c :|c sic 9{c )fc 3{c 3{c 9{c >|c 9|c 9)c :)|c 9}c a(c dfc 3)c 3^ 9f; ^ 3)^ 

% Function accplot.m 

% This function plots each input bode response for the accelerations 

w=logspace(-4,2,100); 
x=-3*ones(size(w)); 

[mag,phase3=bode(as-l*cs,bs(:,5:7),cs,ds(:,5:7),l,w); 
magdb=20*log(mag); 
subplot(311) 

semilogx(w,magdb,w,x,'.0 
grid 

titleC’Bode Response for Ax, Ay, Az') 
axis([le-4 le2 -100 100]) 
pause 
% print 

[mag,phase]=bode(as-l*cs,bs(:,5:7),cs,ds(:,5:7),2,w); 
iaagdb=20*log(mag) ; 
subplot(312) 

semilogx(w,magdb,w,x,^.') 
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grid 


axis([le-4 le2 -100 100]) 

pause 

'/ print 

[mag,phase]=bode(as-l*cs,bs(:,5:7),cs,ds(:,5:7),3,w); 

magdb=20*log(mag); 

subplot(313) 

semilogx(w,magdb,w,x ,’. 

grid 

axis([le-4 le2 -100 100]) 

pause 

’/o print 

+***************************************♦*******♦*♦*♦***************** 
y. Function plotstep.m 

% This function linearizes the Kalman filter with gains and produces a 
y. step response due to accelerations in the x,y, and z axes. 

[af,bf,cf,df]=linmod( ’ kfilnav2’); 

[Y,X,t]=step(af,bf,cf,df); 

plot(t,Y) 
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titlestep response due to inputs in AXjAy,AzO 
pause 
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